From 229593e845c26fc13e5054c332309a3bd7be1af7 Mon Sep 17 00:00:00 2001 From: Aymeric Dujardin Date: Tue, 16 Jul 2024 10:16:16 +0200 Subject: [PATCH] update for ZED SDK 4.1.3 --- .../live/cpp/src/display/GenericDisplay.cpp | 12 +- .../live/python/display/generic_display.py | 5 +- .../map server/assets/css/index.css | 2 + .../map server/assets/leaflet-elipse/LICENSE | 53 ++++ .../assets/leaflet-elipse/README.md | 45 ++++ .../assets/leaflet-elipse/l.ellipse.js | 227 ++++++++++++++++++ .../assets/leaflet-elipse/l.ellipse.min.js | 1 + .../assets/leaflet-elipse/package.json | 15 ++ global localization/map server/index.html | 99 +++++++- .../cpp/src/display/GenericDisplay.cpp | 12 +- .../python/display/generic_display.py | 5 +- .../cpp/src/display/GenericDisplay.cpp | 11 +- .../python/display/generic_display.py | 5 +- .../include/utils.h | 28 ++- .../tensorrt_yolov5-v6-v8_onnx/src/main.cpp | 59 ++++- .../cv_viewer/tracking_viewer.py | 7 +- .../python/pytorch_yolov8/detector.py | 5 +- 17 files changed, 543 insertions(+), 48 deletions(-) create mode 100644 global localization/map server/assets/leaflet-elipse/LICENSE create mode 100644 global localization/map server/assets/leaflet-elipse/README.md create mode 100644 global localization/map server/assets/leaflet-elipse/l.ellipse.js create mode 100644 global localization/map server/assets/leaflet-elipse/l.ellipse.min.js create mode 100644 global localization/map server/assets/leaflet-elipse/package.json diff --git a/global localization/live/cpp/src/display/GenericDisplay.cpp b/global localization/live/cpp/src/display/GenericDisplay.cpp index 7375d124..c9980fc4 100644 --- a/global localization/live/cpp/src/display/GenericDisplay.cpp +++ b/global localization/live/cpp/src/display/GenericDisplay.cpp @@ -39,7 +39,15 @@ void GenericDisplay::updateRawGeoPoseData(sl::GNSSData geo_data) data << longitude; data << ","; data << geo_data.ts.getMilliseconds(); - data << "\n"; + data << ","; + data << geo_data.longitude_std; + data << ","; + data << geo_data.latitude_std; + data << ","; + data << geo_data.altitude_std; + data << ","; + data << geo_data.gnss_status; + data.flush(); // flush will do the same thing than "\n" but without additional character data.close(); } @@ -54,7 +62,7 @@ void GenericDisplay::updateGeoPoseData(sl::GeoPose geo_pose, sl::Timestamp curre data << geo_pose.latlng_coordinates.getLongitude(false); data << ","; data << current_timestamp.getMilliseconds(); - data << "\n"; + data.flush(); // flush will do the same thing than "\n" but without additional character data.close(); // Save the pose in a .kml file diff --git a/global localization/live/python/display/generic_display.py b/global localization/live/python/display/generic_display.py index 3544957d..ec668341 100644 --- a/global localization/live/python/display/generic_display.py +++ b/global localization/live/python/display/generic_display.py @@ -26,7 +26,8 @@ def updateRawGeoPoseData(self, geo_data): # Replace this part with the appropriate sending of data to your IoT system latitude, longitude, _ = geo_data.get_coordinates(False) f = open('../../map server/raw_data.txt', 'w') - f.write("{},{},{}".format(latitude, longitude, geo_data.ts.get_milliseconds())) + f.write("{},{},{},{},{},{},{}".format(latitude, longitude, geo_data.ts.get_milliseconds(),geo_data.longitude_std, geo_data.latitude_std,geo_data.altitude_std, str(geo_data.gnss_status))) + f.flush() except ImportError: print("An exception was raised: the raw geo-pose data was not sent.") @@ -39,7 +40,7 @@ def updateGeoPoseData(self, geo_pose, current_timestamp): .format(geo_pose.latlng_coordinates.get_latitude(False), geo_pose.latlng_coordinates.get_longitude(False), current_timestamp.get_milliseconds())) - + f.flush() gnss_data = {} gnss_data["longitude"] = geo_pose.latlng_coordinates.get_latitude(False) gnss_data["latitude"] = geo_pose.latlng_coordinates.get_latitude(False) diff --git a/global localization/map server/assets/css/index.css b/global localization/map server/assets/css/index.css index 355c74bf..5243c8b8 100644 --- a/global localization/map server/assets/css/index.css +++ b/global localization/map server/assets/css/index.css @@ -131,3 +131,5 @@ input[type="radio"]:hover:checked:after { width: 18px; margin: auto 10px; } +.gnss-status{ +} \ No newline at end of file diff --git a/global localization/map server/assets/leaflet-elipse/LICENSE b/global localization/map server/assets/leaflet-elipse/LICENSE new file mode 100644 index 00000000..490b5c70 --- /dev/null +++ b/global localization/map server/assets/leaflet-elipse/LICENSE @@ -0,0 +1,53 @@ +Apache License + +Version 2.0, January 2004 + +http://www.apache.org/licenses/ + +TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION + +1. Definitions. + +"License" shall mean the terms and conditions for use, reproduction, and distribution as defined by Sections 1 through 9 of this document. + +"Licensor" shall mean the copyright owner or entity authorized by the copyright owner that is granting the License. + +"Legal Entity" shall mean the union of the acting entity and all other entities that control, are controlled by, or are under common control with that entity. For the purposes of this definition, "control" means (i) the power, direct or indirect, to cause the direction or management of such entity, whether by contract or otherwise, or (ii) ownership of fifty percent (50%) or more of the outstanding shares, or (iii) beneficial ownership of such entity. + +"You" (or "Your") shall mean an individual or Legal Entity exercising permissions granted by this License. + +"Source" form shall mean the preferred form for making modifications, including but not limited to software source code, documentation source, and configuration files. + +"Object" form shall mean any form resulting from mechanical transformation or translation of a Source form, including but not limited to compiled object code, generated documentation, and conversions to other media types. + +"Work" shall mean the work of authorship, whether in Source or Object form, made available under the License, as indicated by a copyright notice that is included in or attached to the work (an example is provided in the Appendix below). + +"Derivative Works" shall mean any work, whether in Source or Object form, that is based on (or derived from) the Work and for which the editorial revisions, annotations, elaborations, or other modifications represent, as a whole, an original work of authorship. For the purposes of this License, Derivative Works shall not include works that remain separable from, or merely link (or bind by name) to the interfaces of, the Work and Derivative Works thereof. + +"Contribution" shall mean any work of authorship, including the original version of the Work and any modifications or additions to that Work or Derivative Works thereof, that is intentionally submitted to Licensor for inclusion in the Work by the copyright owner or by an individual or Legal Entity authorized to submit on behalf of the copyright owner. For the purposes of this definition, "submitted" means any form of electronic, verbal, or written communication sent to the Licensor or its representatives, including but not limited to communication on electronic mailing lists, source code control systems, and issue tracking systems that are managed by, or on behalf of, the Licensor for the purpose of discussing and improving the Work, but excluding communication that is conspicuously marked or otherwise designated in writing by the copyright owner as "Not a Contribution." + +"Contributor" shall mean Licensor and any individual or Legal Entity on behalf of whom a Contribution has been received by Licensor and subsequently incorporated within the Work. + +2. Grant of Copyright License. Subject to the terms and conditions of this License, each Contributor hereby grants to You a perpetual, worldwide, non-exclusive, no-charge, royalty-free, irrevocable copyright license to reproduce, prepare Derivative Works of, publicly display, publicly perform, sublicense, and distribute the Work and such Derivative Works in Source or Object form. + +3. Grant of Patent License. Subject to the terms and conditions of this License, each Contributor hereby grants to You a perpetual, worldwide, non-exclusive, no-charge, royalty-free, irrevocable (except as stated in this section) patent license to make, have made, use, offer to sell, sell, import, and otherwise transfer the Work, where such license applies only to those patent claims licensable by such Contributor that are necessarily infringed by their Contribution(s) alone or by combination of their Contribution(s) with the Work to which such Contribution(s) was submitted. If You institute patent litigation against any entity (including a cross-claim or counterclaim in a lawsuit) alleging that the Work or a Contribution incorporated within the Work constitutes direct or contributory patent infringement, then any patent licenses granted to You under this License for that Work shall terminate as of the date such litigation is filed. + +4. Redistribution. You may reproduce and distribute copies of the Work or Derivative Works thereof in any medium, with or without modifications, and in Source or Object form, provided that You meet the following conditions: + +You must give any other recipients of the Work or Derivative Works a copy of this License; and +You must cause any modified files to carry prominent notices stating that You changed the files; and +You must retain, in the Source form of any Derivative Works that You distribute, all copyright, patent, trademark, and attribution notices from the Source form of the Work, excluding those notices that do not pertain to any part of the Derivative Works; and +If the Work includes a "NOTICE" text file as part of its distribution, then any Derivative Works that You distribute must include a readable copy of the attribution notices contained within such NOTICE file, excluding those notices that do not pertain to any part of the Derivative Works, in at least one of the following places: within a NOTICE text file distributed as part of the Derivative Works; within the Source form or documentation, if provided along with the Derivative Works; or, within a display generated by the Derivative Works, if and wherever such third-party notices normally appear. The contents of the NOTICE file are for informational purposes only and do not modify the License. You may add Your own attribution notices within Derivative Works that You distribute, alongside or as an addendum to the NOTICE text from the Work, provided that such additional attribution notices cannot be construed as modifying the License. + +You may add Your own copyright statement to Your modifications and may provide additional or different license terms and conditions for use, reproduction, or distribution of Your modifications, or for any such Derivative Works as a whole, provided Your use, reproduction, and distribution of the Work otherwise complies with the conditions stated in this License. +5. Submission of Contributions. Unless You explicitly state otherwise, any Contribution intentionally submitted for inclusion in the Work by You to the Licensor shall be under the terms and conditions of this License, without any additional terms or conditions. Notwithstanding the above, nothing herein shall supersede or modify the terms of any separate license agreement you may have executed with Licensor regarding such Contributions. + +6. Trademarks. This License does not grant permission to use the trade names, trademarks, service marks, or product names of the Licensor, except as required for reasonable and customary use in describing the origin of the Work and reproducing the content of the NOTICE file. + +7. Disclaimer of Warranty. Unless required by applicable law or agreed to in writing, Licensor provides the Work (and each Contributor provides its Contributions) on an "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied, including, without limitation, any warranties or conditions of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A PARTICULAR PURPOSE. You are solely responsible for determining the appropriateness of using or redistributing the Work and assume any risks associated with Your exercise of permissions under this License. + +8. Limitation of Liability. In no event and under no legal theory, whether in tort (including negligence), contract, or otherwise, unless required by applicable law (such as deliberate and grossly negligent acts) or agreed to in writing, shall any Contributor be liable to You for damages, including any direct, indirect, special, incidental, or consequential damages of any character arising as a result of this License or out of the use or inability to use the Work (including but not limited to damages for loss of goodwill, work stoppage, computer failure or malfunction, or any and all other commercial damages or losses), even if such Contributor has been advised of the possibility of such damages. + +9. Accepting Warranty or Additional Liability. While redistributing the Work or Derivative Works thereof, You may choose to offer, and charge a fee for, acceptance of support, warranty, indemnity, or other liability obligations and/or rights consistent with this License. However, in accepting such obligations, You may act only on Your own behalf and on Your sole responsibility, not on behalf of any other Contributor, and only if You agree to indemnify, defend, and hold each Contributor harmless for any liability incurred by, or claims asserted against, such Contributor by reason of your accepting any such warranty or additional liability. + +END OF TERMS AND CONDITIONS \ No newline at end of file diff --git a/global localization/map server/assets/leaflet-elipse/README.md b/global localization/map server/assets/leaflet-elipse/README.md new file mode 100644 index 00000000..7745060a --- /dev/null +++ b/global localization/map server/assets/leaflet-elipse/README.md @@ -0,0 +1,45 @@ +Leaflet.ellipse +=============== + +A basic ellipse type for [Leaflet](http://leafletjs.com), a JS +library for interactive maps. Allows specification of semi-major and +semi-minor axis as well as a tilt degrees from west. + +*Supports Leaflet 1.0.0 or newer.* + +## Supported browsers + +* Internet Explorer 10+ +* Google Chrome +* Safari +* Firefox 17+ + +## Demo + +A demo is available on the Github Pages webpage for Leaflet.ellipse [here](http://jdfergason.github.io/Leaflet.Ellipse/). + +## Usage + +Include the ellipse javasript file: + + + +After instantiating the map create a new ellipse. + + var ellipse = L.ellipse([51.5, -0.09], [500, 100], 90).addTo(map); + +## API + +*Factory method* + + L.ellipse( latlng, radii, tilt, + options? ) + + * latlng - The position of the center of the ellipse. + * radii - The semi-major and semi-minor axis in meters + * tilt - The rotation of the ellipse in degrees from west + * options - Options dictionary to pass to L.Path + +## License + +This code is provided under the Apache 2.0 license. diff --git a/global localization/map server/assets/leaflet-elipse/l.ellipse.js b/global localization/map server/assets/leaflet-elipse/l.ellipse.js new file mode 100644 index 00000000..f4d7ba36 --- /dev/null +++ b/global localization/map server/assets/leaflet-elipse/l.ellipse.js @@ -0,0 +1,227 @@ +/** + * Copyright 2014 JD Fergason + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + + +L.SVG.include ({ + _updateEllipse: function (layer) { + var c = layer._point, + rx = layer._radiusX, + ry = layer._radiusY, + phi = layer._tiltDeg, + endPoint = layer._endPointParams; + + var d = 'M' + endPoint.x0 + ',' + endPoint.y0 + + 'A' + rx + ',' + ry + ',' + phi + ',' + + endPoint.largeArc + ',' + endPoint.sweep + ',' + + endPoint.x1 + ',' + endPoint.y1 + ' z'; + this._setPath(layer, d); + } +}); + +L.Canvas.include ({ + _updateEllipse: function (layer) { + if (layer._empty()) { return; } + + var p = layer._point, + ctx = this._ctx, + r = layer._radiusX, + s = (layer._radiusY || r) / r; + + this._drawnLayers[layer._leaflet_id] = layer; + + ctx.save(); + + ctx.translate(p.x, p.y); + if (layer._tilt !== 0) { + ctx.rotate( layer._tilt ); + } + if (s !== 1) { + ctx.scale(1, s); + } + + ctx.beginPath(); + ctx.arc(0, 0, r, 0, Math.PI * 2); + ctx.restore(); + + this._fillStroke(ctx, layer); + }, +}); + +L.Ellipse = L.Path.extend({ + + options: { + fill: true, + startAngle: 0, + endAngle: 359.9 + }, + + initialize: function (latlng, radii, tilt, options) { + + L.setOptions(this, options); + this._latlng = L.latLng(latlng); + + if (tilt) { + this._tiltDeg = tilt; + } else { + this._tiltDeg = 0; + } + + if (radii) { + this._mRadiusX = radii[0]; + this._mRadiusY = radii[1]; + } + }, + + setRadius: function (radii) { + this._mRadiusX = radii[0]; + this._mRadiusY = radii[1]; + return this.redraw(); + }, + + getRadius: function () { + return new L.point(this._mRadiusX, this._mRadiusY); + }, + + setTilt: function (tilt) { + this._tiltDeg = tilt; + return this.redraw(); + }, + + getBounds: function () { + // TODO respect tilt (bounds are too big) + var lngRadius = this._getLngRadius(), + latRadius = this._getLatRadius(), + latlng = this._latlng; + + return new L.LatLngBounds( + [latlng.lat - latRadius, latlng.lng - lngRadius], + [latlng.lat + latRadius, latlng.lng + lngRadius]); + }, + + // @method setLatLng(latLng: LatLng): this + // Sets the position of a circle marker to a new location. + setLatLng: function (latlng) { + this._latlng = L.latLng(latlng); + this.redraw(); + return this.fire('move', {latlng: this._latlng}); + }, + + // @method getLatLng(): LatLng + // Returns the current geographical position of the circle marker + getLatLng: function () { + return this._latlng; + }, + + setStyle: L.Path.prototype.setStyle, + + _project: function () { + var lngRadius = this._getLngRadius(), + latRadius = this._getLatRadius(), + latlng = this._latlng, + pointLeft = this._map.latLngToLayerPoint([latlng.lat, latlng.lng - lngRadius]), + pointBelow = this._map.latLngToLayerPoint([latlng.lat - latRadius, latlng.lng]); + + this._point = this._map.latLngToLayerPoint(latlng); + this._radiusX = Math.max(this._point.x - pointLeft.x, 1); + this._radiusY = Math.max(pointBelow.y - this._point.y, 1); + this._tilt = Math.PI * this._tiltDeg / 180; + this._endPointParams = this._centerPointToEndPoint(); + this._updateBounds(); + }, + + _updateBounds: function () { + // http://math.stackexchange.com/questions/91132/how-to-get-the-limits-of-rotated-ellipse + var sin = Math.sin(this._tilt); + var cos = Math.cos(this._tilt); + var sinSquare = sin * sin; + var cosSquare = cos * cos; + var aSquare = this._radiusX * this._radiusX; + var bSquare = this._radiusY * this._radiusY; + var halfWidth = Math.sqrt(aSquare*cosSquare+bSquare*sinSquare); + var halfHeight = Math.sqrt(aSquare*sinSquare+bSquare*cosSquare); + var w = this._clickTolerance(); + var p = [halfWidth + w, halfHeight + w]; + this._pxBounds = new L.Bounds(this._point.subtract(p), this._point.add(p)); + }, + + _update: function () { + if (this._map) { + this._updatePath(); + } + }, + + _updatePath: function () { + this._renderer._updateEllipse(this); + }, + + _getLatRadius: function () { + return (this._mRadiusY / 40075017) * 360; + }, + + _getLngRadius: function () { + return ((this._mRadiusX / 40075017) * 360) / Math.cos((Math.PI / 180) * this._latlng.lat); + }, + + _centerPointToEndPoint: function () { + // Convert between center point parameterization of an ellipse + // too SVG's end-point and sweep parameters. This is an + // adaptation of the perl code found here: + // http://commons.oreilly.com/wiki/index.php/SVG_Essentials/Paths + var c = this._point, + rx = this._radiusX, + ry = this._radiusY, + theta2 = (this.options.startAngle + this.options.endAngle) * (Math.PI / 180), + theta1 = this.options.startAngle * (Math.PI / 180), + delta = this.options.endAngle, + phi = this._tiltDeg * (Math.PI / 180); + + // Determine start and end-point coordinates + var x0 = c.x + Math.cos(phi) * rx * Math.cos(theta1) + + Math.sin(-phi) * ry * Math.sin(theta1); + var y0 = c.y + Math.sin(phi) * rx * Math.cos(theta1) + + Math.cos(phi) * ry * Math.sin(theta1); + + var x1 = c.x + Math.cos(phi) * rx * Math.cos(theta2) + + Math.sin(-phi) * ry * Math.sin(theta2); + var y1 = c.y + Math.sin(phi) * rx * Math.cos(theta2) + + Math.cos(phi) * ry * Math.sin(theta2); + + var largeArc = (delta > 180) ? 1 : 0; + var sweep = (delta > 0) ? 1 : 0; + + return {'x0': x0, 'y0': y0, 'tilt': phi, 'largeArc': largeArc, + 'sweep': sweep, 'x1': x1, 'y1': y1}; + }, + + _empty: function () { + return this._radiusX && this._radiusY && !this._renderer._bounds.intersects(this._pxBounds); + }, + + _containsPoint : function (p) { + // http://stackoverflow.com/questions/7946187/point-and-ellipse-rotated-position-test-algorithm + var sin = Math.sin(this._tilt); + var cos = Math.cos(this._tilt); + var dx = p.x - this._point.x; + var dy = p.y - this._point.y; + var sumA = cos * dx + sin * dy; + var sumB = sin * dx - cos * dy; + return sumA * sumA / (this._radiusX * this._radiusX) + sumB * sumB / (this._radiusY * this._radiusY) <= 1; + } +}); + +L.ellipse = function (latlng, radii, tilt, options) { + return new L.Ellipse(latlng, radii, tilt, options); +}; diff --git a/global localization/map server/assets/leaflet-elipse/l.ellipse.min.js b/global localization/map server/assets/leaflet-elipse/l.ellipse.min.js new file mode 100644 index 00000000..dbe75cc4 --- /dev/null +++ b/global localization/map server/assets/leaflet-elipse/l.ellipse.min.js @@ -0,0 +1 @@ +L.SVG.include({_updateEllipse:function(a){var c=(a._point,a._radiusX),d=a._radiusY,e=a._tiltDeg,f=a._endPointParams,g="M"+f.x0+","+f.y0+"A"+c+","+d+","+e+","+f.largeArc+","+f.sweep+","+f.x1+","+f.y1+" z";this._setPath(a,g)}}),L.Canvas.include({_updateEllipse:function(a){if(!a._empty()){var b=a._point,c=this._ctx,d=a._radiusX,e=(a._radiusY||d)/d;this._drawnLayers[a._leaflet_id]=a,c.save(),c.translate(b.x,b.y),0!==a._tilt&&c.rotate(a._tilt),1!==e&&c.scale(1,e),c.beginPath(),c.arc(0,0,d,0,2*Math.PI),c.restore(),this._fillStroke(c,a)}}}),L.Ellipse=L.Path.extend({options:{fill:!0,startAngle:0,endAngle:359.9},initialize:function(a,b,c,d){L.setOptions(this,d),this._latlng=L.latLng(a),c?this._tiltDeg=c:this._tiltDeg=0,b&&(this._mRadiusX=b[0],this._mRadiusY=b[1])},setRadius:function(a){return this._mRadiusX=a[0],this._mRadiusY=a[1],this.redraw()},getRadius:function(){return new L.point(this._mRadiusX,this._mRadiusY)},setTilt:function(a){return this._tiltDeg=a,this.redraw()},getBounds:function(){var a=this._getLngRadius(),b=this._getLatRadius(),c=this._latlng;return new L.LatLngBounds([c.lat-b,c.lng-a],[c.lat+b,c.lng+a])},setLatLng:function(a){return this._latlng=L.latLng(a),this.redraw(),this.fire("move",{latlng:this._latlng})},getLatLng:function(){return this._latlng},setStyle:L.Path.prototype.setStyle,_project:function(){var a=this._getLngRadius(),b=this._getLatRadius(),c=this._latlng,d=this._map.latLngToLayerPoint([c.lat,c.lng-a]),e=this._map.latLngToLayerPoint([c.lat-b,c.lng]);this._point=this._map.latLngToLayerPoint(c),this._radiusX=Math.max(this._point.x-d.x,1),this._radiusY=Math.max(e.y-this._point.y,1),this._tilt=Math.PI*this._tiltDeg/180,this._endPointParams=this._centerPointToEndPoint(),this._updateBounds()},_updateBounds:function(){var a=Math.sin(this._tilt),b=Math.cos(this._tilt),c=a*a,d=b*b,e=this._radiusX*this._radiusX,f=this._radiusY*this._radiusY,g=Math.sqrt(e*d+f*c),h=Math.sqrt(e*c+f*d),i=this._clickTolerance(),j=[g+i,h+i];this._pxBounds=new L.Bounds(this._point.subtract(j),this._point.add(j))},_update:function(){this._map&&this._updatePath()},_updatePath:function(){this._renderer._updateEllipse(this)},_getLatRadius:function(){return this._mRadiusY/40075017*360},_getLngRadius:function(){return this._mRadiusX/40075017*360/Math.cos(Math.PI/180*this._latlng.lat)},_centerPointToEndPoint:function(){var a=this._point,b=this._radiusX,c=this._radiusY,d=(this.options.startAngle+this.options.endAngle)*(Math.PI/180),e=this.options.startAngle*(Math.PI/180),f=this.options.endAngle,g=this._tiltDeg*(Math.PI/180),h=a.x+Math.cos(g)*b*Math.cos(e)+Math.sin(-g)*c*Math.sin(e),i=a.y+Math.sin(g)*b*Math.cos(e)+Math.cos(g)*c*Math.sin(e),j=a.x+Math.cos(g)*b*Math.cos(d)+Math.sin(-g)*c*Math.sin(d),k=a.y+Math.sin(g)*b*Math.cos(d)+Math.cos(g)*c*Math.sin(d),l=f>180?1:0,m=f>0?1:0;return{x0:h,y0:i,tilt:g,largeArc:l,sweep:m,x1:j,y1:k}},_empty:function(){return this._radiusX&&this._radiusY&&!this._renderer._bounds.intersects(this._pxBounds)},_containsPoint:function(a){var b=Math.sin(this._tilt),c=Math.cos(this._tilt),d=a.x-this._point.x,e=a.y-this._point.y,f=c*d+b*e,g=b*d-c*e;return f*f/(this._radiusX*this._radiusX)+g*g/(this._radiusY*this._radiusY)<=1}}),L.ellipse=function(a,b,c,d){return new L.Ellipse(a,b,c,d)}; \ No newline at end of file diff --git a/global localization/map server/assets/leaflet-elipse/package.json b/global localization/map server/assets/leaflet-elipse/package.json new file mode 100644 index 00000000..8cf1a1be --- /dev/null +++ b/global localization/map server/assets/leaflet-elipse/package.json @@ -0,0 +1,15 @@ +{ + "name": "leaflet-ellipse", + "version": "0.9.1", + "description": "A basic ellipse type for Leaflet, a JS library for interactive maps.", + "main": "l.ellipse.js", + "repository": { + "type": "git", + "url": "git://github.com/jdfergason/Leaflet.Ellipse" + }, + "keywords": [ + "gis", + "map" + ], + "license": "Apache 2.0" +} diff --git a/global localization/map server/index.html b/global localization/map server/index.html index 8dffd195..373e3ea9 100644 --- a/global localization/map server/index.html +++ b/global localization/map server/index.html @@ -11,6 +11,7 @@ + @@ -19,6 +20,12 @@
diff --git a/global localization/playback/cpp/src/display/GenericDisplay.cpp b/global localization/playback/cpp/src/display/GenericDisplay.cpp index 7375d124..c9980fc4 100644 --- a/global localization/playback/cpp/src/display/GenericDisplay.cpp +++ b/global localization/playback/cpp/src/display/GenericDisplay.cpp @@ -39,7 +39,15 @@ void GenericDisplay::updateRawGeoPoseData(sl::GNSSData geo_data) data << longitude; data << ","; data << geo_data.ts.getMilliseconds(); - data << "\n"; + data << ","; + data << geo_data.longitude_std; + data << ","; + data << geo_data.latitude_std; + data << ","; + data << geo_data.altitude_std; + data << ","; + data << geo_data.gnss_status; + data.flush(); // flush will do the same thing than "\n" but without additional character data.close(); } @@ -54,7 +62,7 @@ void GenericDisplay::updateGeoPoseData(sl::GeoPose geo_pose, sl::Timestamp curre data << geo_pose.latlng_coordinates.getLongitude(false); data << ","; data << current_timestamp.getMilliseconds(); - data << "\n"; + data.flush(); // flush will do the same thing than "\n" but without additional character data.close(); // Save the pose in a .kml file diff --git a/global localization/playback/python/display/generic_display.py b/global localization/playback/python/display/generic_display.py index 3544957d..ec668341 100644 --- a/global localization/playback/python/display/generic_display.py +++ b/global localization/playback/python/display/generic_display.py @@ -26,7 +26,8 @@ def updateRawGeoPoseData(self, geo_data): # Replace this part with the appropriate sending of data to your IoT system latitude, longitude, _ = geo_data.get_coordinates(False) f = open('../../map server/raw_data.txt', 'w') - f.write("{},{},{}".format(latitude, longitude, geo_data.ts.get_milliseconds())) + f.write("{},{},{},{},{},{},{}".format(latitude, longitude, geo_data.ts.get_milliseconds(),geo_data.longitude_std, geo_data.latitude_std,geo_data.altitude_std, str(geo_data.gnss_status))) + f.flush() except ImportError: print("An exception was raised: the raw geo-pose data was not sent.") @@ -39,7 +40,7 @@ def updateGeoPoseData(self, geo_pose, current_timestamp): .format(geo_pose.latlng_coordinates.get_latitude(False), geo_pose.latlng_coordinates.get_longitude(False), current_timestamp.get_milliseconds())) - + f.flush() gnss_data = {} gnss_data["longitude"] = geo_pose.latlng_coordinates.get_latitude(False) gnss_data["latitude"] = geo_pose.latlng_coordinates.get_latitude(False) diff --git a/global localization/recording/cpp/src/display/GenericDisplay.cpp b/global localization/recording/cpp/src/display/GenericDisplay.cpp index 7375d124..2c26fb23 100644 --- a/global localization/recording/cpp/src/display/GenericDisplay.cpp +++ b/global localization/recording/cpp/src/display/GenericDisplay.cpp @@ -34,12 +34,21 @@ void GenericDisplay::updateRawGeoPoseData(sl::GNSSData geo_data) ofstream data; data.open ("../../../map server/raw_data.txt"); data << std::fixed << std::setprecision(17); + data << std::fixed << std::setprecision(17); data << latitude; data << ","; data << longitude; data << ","; data << geo_data.ts.getMilliseconds(); - data << "\n"; + data << ","; + data << geo_data.longitude_std; + data << ","; + data << geo_data.latitude_std; + data << ","; + data << geo_data.altitude_std; + data << ","; + data << geo_data.gnss_status; + data.flush(); // flush will do the same thing than "\n" but without additional character data.close(); } diff --git a/global localization/recording/python/display/generic_display.py b/global localization/recording/python/display/generic_display.py index bb77db05..d012fe17 100644 --- a/global localization/recording/python/display/generic_display.py +++ b/global localization/recording/python/display/generic_display.py @@ -26,7 +26,8 @@ def updateRawGeoPoseData(self, geo_data): # Replace this part with the appropriate sending of data to your IoT system latitude, longitude, _ = geo_data.get_coordinates(False) f = open('../../map server/raw_data.txt', 'w') - f.write("{},{},{}".format(latitude, longitude, geo_data.ts.get_milliseconds())) + f.write("{},{},{},{},{},{},{}".format(latitude, longitude, geo_data.ts.get_milliseconds(),geo_data.longitude_std, geo_data.latitude_std,geo_data.altitude_std, str(geo_data.gnss_status))) + f.flush() except ImportError: print("An exception was raised: the raw geo-pose data was not sent.") @@ -39,7 +40,7 @@ def updateGeoPoseData(self, geo_pose, current_timestamp): .format(geo_pose.latlng_coordinates.get_latitude(False), geo_pose.latlng_coordinates.get_longitude(False), current_timestamp.get_milliseconds())) - + f.flush() except ImportError: print("An exception was raised: the geo-pose data was not sent.") diff --git a/object detection/custom detector/cpp/tensorrt_yolov5-v6-v8_onnx/include/utils.h b/object detection/custom detector/cpp/tensorrt_yolov5-v6-v8_onnx/include/utils.h index 14094e55..c7bb2779 100644 --- a/object detection/custom detector/cpp/tensorrt_yolov5-v6-v8_onnx/include/utils.h +++ b/object detection/custom detector/cpp/tensorrt_yolov5-v6-v8_onnx/include/utils.h @@ -29,18 +29,26 @@ static inline cv::Mat preprocess_img(cv::Mat& img, int input_w, int input_h) { return out; } -float const id_colors[5][3] = { - { 232.0f, 176.0f, 59.0f}, - { 175.0f, 208.0f, 25.0f}, - { 102.0f, 205.0f, 105.0f}, - { 185.0f, 0.0f, 255.0f}, - { 99.0f, 107.0f, 252.0f} -}; +std::vector> const CLASS_COLORS = { + {0, 114, 189}, {217, 83, 25}, {237, 177, 32}, {126, 47, 142}, {119, 172, 48}, {77, 190, 238}, + {162, 20, 47}, {76, 76, 76}, {153, 153, 153}, {255, 0, 0}, {255, 128, 0}, {191, 191, 0}, + {0, 255, 0}, {0, 0, 255}, {170, 0, 255}, {85, 85, 0}, {85, 170, 0}, {85, 255, 0}, + {170, 85, 0}, {170, 170, 0}, {170, 255, 0}, {255, 85, 0}, {255, 170, 0}, {255, 255, 0}, + {0, 85, 128}, {0, 170, 128}, {0, 255, 128}, {85, 0, 128}, {85, 85, 128}, {85, 170, 128}, + {85, 255, 128}, {170, 0, 128}, {170, 85, 128}, {170, 170, 128}, {170, 255, 128}, {255, 0, 128}, + {255, 85, 128}, {255, 170, 128}, {255, 255, 128}, {0, 85, 255}, {0, 170, 255}, {0, 255, 255}, + {85, 0, 255}, {85, 85, 255}, {85, 170, 255}, {85, 255, 255}, {170, 0, 255}, {170, 85, 255}, + {170, 170, 255}, {170, 255, 255}, {255, 0, 255}, {255, 85, 255}, {255, 170, 255}, {85, 0, 0}, + {128, 0, 0}, {170, 0, 0}, {212, 0, 0}, {255, 0, 0}, {0, 43, 0}, {0, 85, 0}, + {0, 128, 0}, {0, 170, 0}, {0, 212, 0}, {0, 255, 0}, {0, 0, 43}, {0, 0, 85}, + {0, 0, 128}, {0, 0, 170}, {0, 0, 212}, {0, 0, 255}, {0, 0, 0}, {36, 36, 36}, + {73, 73, 73}, {109, 109, 109}, {146, 146, 146}, {182, 182, 182}, {219, 219, 219}, {0, 114, 189}, + {80, 183, 189}, {128, 128, 0}}; inline cv::Scalar generateColorID_u(int idx) { if (idx < 0) return cv::Scalar(236, 184, 36, 255); - int color_idx = idx % 5; - return cv::Scalar(id_colors[color_idx][0], id_colors[color_idx][1], id_colors[color_idx][2], 255); + int color_idx = idx % CLASS_COLORS.size(); + return cv::Scalar(CLASS_COLORS[color_idx][0], CLASS_COLORS[color_idx][1], CLASS_COLORS[color_idx][2], 255); } inline sl::float4 generateColorID_f(int idx) { @@ -102,7 +110,7 @@ inline void drawVerticalLine( cv::line(left_display, pt4, end_pt, clr, thickness); } -inline cv::Mat slMat2cvMat(sl::Mat& input) { +inline cv::Mat slMat2cvMat(sl::Mat const& input) { // Mapping between MAT_TYPE and CV_TYPE int cv_type = -1; switch (input.getDataType()) { diff --git a/object detection/custom detector/cpp/tensorrt_yolov5-v6-v8_onnx/src/main.cpp b/object detection/custom detector/cpp/tensorrt_yolov5-v6-v8_onnx/src/main.cpp index eeb0493c..d9985f46 100644 --- a/object detection/custom detector/cpp/tensorrt_yolov5-v6-v8_onnx/src/main.cpp +++ b/object detection/custom detector/cpp/tensorrt_yolov5-v6-v8_onnx/src/main.cpp @@ -15,6 +15,42 @@ using namespace nvinfer1; #define NMS_THRESH 0.4 #define CONF_THRESH 0.3 +static void draw_objects(cv::Mat const& image, + cv::Mat &res, + sl::Objects const& objs, + std::vector> const& colors) +{ + res = image.clone(); + cv::Mat mask{image.clone()}; + for (sl::ObjectData const& obj : objs.object_list) { + size_t const idx_color{obj.id % colors.size()}; + cv::Scalar const color{cv::Scalar(colors[idx_color][0U], colors[idx_color][1U], colors[idx_color][2U])}; + + cv::Rect const rect{static_cast(obj.bounding_box_2d[0U].x), + static_cast(obj.bounding_box_2d[0U].y), + static_cast(obj.bounding_box_2d[1U].x - obj.bounding_box_2d[0U].x), + static_cast(obj.bounding_box_2d[2U].y - obj.bounding_box_2d[0U].y)}; + cv::rectangle(res, rect, color, 2); + + char text[256U]; + sprintf(text, "Class %d - %.1f%%", obj.raw_label, obj.confidence); + if (obj.mask.isInit() && obj.mask.getWidth() > 0U && obj.mask.getHeight() > 0U) { + const cv::Mat obj_mask = slMat2cvMat(obj.mask); + mask(rect).setTo(color, obj_mask); + } + + int baseLine{0}; + cv::Size const label_size{cv::getTextSize(text, cv::FONT_HERSHEY_SIMPLEX, 0.4, 1, &baseLine)}; + + int const x{rect.x}; + int const y{std::min(rect.y + 1, res.rows)}; + + cv::rectangle(res, cv::Rect(x, y, label_size.width, label_size.height + baseLine), {0, 0, 255}, -1); + cv::putText(res, text, cv::Point(x, y + label_size.height), cv::FONT_HERSHEY_SIMPLEX, 0.4, {255, 255, 255}, 1); + } + cv::addWeighted(res, 0.5, mask, 0.8, 1, res); +} + void print(std::string msg_prefix, sl::ERROR_CODE err_code, std::string msg_suffix) { std::cout << "[Sample] "; if (err_code != sl::ERROR_CODE::SUCCESS) @@ -74,7 +110,7 @@ int main(int argc, char** argv) { init_parameters.depth_mode = sl::DEPTH_MODE::ULTRA; init_parameters.coordinate_system = sl::COORDINATE_SYSTEM::RIGHT_HANDED_Y_UP; // OpenGL's coordinate system is right_handed - if (argc > 1) { + if (argc > 2) { std::string zed_opt = argv[2]; if (zed_opt.find(".svo") != std::string::npos) init_parameters.input.setFromSVOFile(zed_opt.c_str()); @@ -89,7 +125,7 @@ int main(int argc, char** argv) { // Custom OD sl::ObjectDetectionParameters detection_parameters; detection_parameters.enable_tracking = true; - detection_parameters.enable_segmentation = false; // designed to give person pixel mask + detection_parameters.enable_segmentation = false; // designed to give person pixel mask with internal OD detection_parameters.detection_model = sl::OBJECT_DETECTION_MODEL::CUSTOM_BOX_OBJECTS; returned_state = zed.enableObjectDetection(detection_parameters); if (returned_state != sl::ERROR_CODE::SUCCESS) { @@ -156,23 +192,20 @@ int main(int argc, char** argv) { // Send the custom detected boxes to the ZED zed.ingestCustomBoxObjects(objects_in); - - // Displaying 'raw' objects - for (size_t j = 0; j < detections.size(); j++) { - cv::Rect r = get_rect(detections[j].box); - cv::rectangle(left_cv, r, cv::Scalar(0x27, 0xC1, 0x36), 2); - cv::putText(left_cv, std::to_string((int) detections[j].label), cv::Point(r.x, r.y - 1), cv::FONT_HERSHEY_PLAIN, 1.2, cv::Scalar(0xFF, 0xFF, 0xFF), 2); - } - cv::imshow("Objects", left_cv); - cv::waitKey(10); - - // Retrieve the tracked objects, with 2D and 3D attributes zed.retrieveObjects(objects, objectTracker_parameters_rt); + // GL Viewer zed.retrieveMeasure(point_cloud, sl::MEASURE::XYZRGBA, sl::MEM::GPU, pc_resolution); zed.getPosition(cam_w_pose, sl::REFERENCE_FRAME::WORLD); viewer.updateData(point_cloud, objects.object_list, cam_w_pose.pose_data); + + // Displaying the SDK objects + draw_objects(left_cv, left_cv, objects, CLASS_COLORS); + cv::imshow("ZED retrieved Objects", left_cv); + int const key{cv::waitKey(10)}; + if (key == 'q' || key == 'Q' || key == 27) + break; } } diff --git a/object detection/custom detector/python/pytorch_yolov8/cv_viewer/tracking_viewer.py b/object detection/custom detector/python/pytorch_yolov8/cv_viewer/tracking_viewer.py index cfc4d4bd..dcdf51a5 100644 --- a/object detection/custom detector/python/pytorch_yolov8/cv_viewer/tracking_viewer.py +++ b/object detection/custom detector/python/pytorch_yolov8/cv_viewer/tracking_viewer.py @@ -54,10 +54,11 @@ def render_2D(left_display, img_scale, objects, is_tracking_on): # Scaled ROI roi_height = int(top_right_corner[0] - top_left_corner[0]) roi_width = int(bottom_left_corner[1] - top_left_corner[1]) - overlay_roi = overlay[int(top_left_corner[1]):int(top_left_corner[1] + roi_width) - , int(top_left_corner[0]):int(top_left_corner[0] + roi_height)] + overlay_roi = overlay[int(top_left_corner[1]):int(top_left_corner[1] + roi_width), + int(top_left_corner[0]):int(top_left_corner[0] + roi_height)] - overlay_roi[:, :, :] = base_color + if obj.mask.is_init(): + overlay_roi[obj.mask.numpy() != 0] = base_color # Display Object label as text position_image = get_image_position(obj.bounding_box_2d, img_scale) diff --git a/object detection/custom detector/python/pytorch_yolov8/detector.py b/object detection/custom detector/python/pytorch_yolov8/detector.py index 47a4ceb7..b2bf540c 100644 --- a/object detection/custom detector/python/pytorch_yolov8/detector.py +++ b/object detection/custom detector/python/pytorch_yolov8/detector.py @@ -123,6 +123,7 @@ def main(): obj_param = sl.ObjectDetectionParameters() obj_param.detection_model = sl.OBJECT_DETECTION_MODEL.CUSTOM_BOX_OBJECTS obj_param.enable_tracking = True + obj_param.enable_segmentation = False # designed to give person pixel mask with internal OD zed.enable_object_detection(obj_param) objects = sl.Objects() @@ -190,7 +191,7 @@ def main(): cv2.imshow("ZED | 2D View and Birds View", global_image) key = cv2.waitKey(10) - if key == 27: + if key == 27 or key == ord('q') or key == ord('Q'): exit_signal = True else: exit_signal = True @@ -203,7 +204,7 @@ def main(): if __name__ == '__main__': parser = argparse.ArgumentParser() parser.add_argument('--weights', type=str, default='yolov8m.pt', help='model.pt path(s)') - parser.add_argument('--svo', type=str, default=None, help='optional svo file') + parser.add_argument('--svo', type=str, default=None, help='optional svo file, if not passed, use the plugged camera instead') parser.add_argument('--img_size', type=int, default=416, help='inference size (pixels)') parser.add_argument('--conf_thres', type=float, default=0.4, help='object confidence threshold') opt = parser.parse_args()