From ae26c14f60bc17221ef7ea6b0274219408c80e54 Mon Sep 17 00:00:00 2001 From: Jose Luis Blanco-Claraco Date: Thu, 30 Nov 2023 03:27:45 +0100 Subject: [PATCH] Progress finishing CObservationRotatingScan --- .../main_show_selected_object.cpp | 31 +++++++-- .../mrpt/obs/CObservationRotatingScan.h | 17 +++-- .../maps/src/obs/CObservationRotatingScan.cpp | 40 +++++++----- .../include/mrpt/ros2bridge/point_cloud2.h | 10 ++- libs/ros2bridge/src/point_cloud2.cpp | 65 +++++++++++++++---- 5 files changed, 119 insertions(+), 44 deletions(-) diff --git a/apps/RawLogViewer/main_show_selected_object.cpp b/apps/RawLogViewer/main_show_selected_object.cpp index faf279b7e4..c645dc6759 100644 --- a/apps/RawLogViewer/main_show_selected_object.cpp +++ b/apps/RawLogViewer/main_show_selected_object.cpp @@ -471,15 +471,34 @@ void xRawLogViewerFrame::SelectObjectInTreeView( // Get range image as bitmap: // --------------------------- - mrpt::img::CImage img_range; - img_range.setFromMatrix(obs->rangeImage, false); + { + mrpt::img::CImage img_range; + + float maxActualRange = obs->rangeImage.maxCoeff(); + if (maxActualRange == 0) maxActualRange = 1.0f; + + img_range.setFromMatrix( + obs->rangeImage.asEigen().cast() * + (1.0f / maxActualRange), + true /*already in [0,1]*/); + + showImageInGLView(*bmpObsStereoLeft, img_range); + } + + if (!obs->intensityImage.empty()) + { + mrpt::img::CImage img_intensity; - showImageInGLView(*bmpObsStereoLeft, img_range); + float maxActualInt = obs->intensityImage.maxCoeff(); + if (maxActualInt == 0) maxActualInt = 1.0f; - mrpt::img::CImage img_intensity; - img_intensity.setFromMatrix(obs->intensityImage, false); + img_intensity.setFromMatrix( + obs->intensityImage.asEigen().cast() * + (1.0f / maxActualInt), + true /*already in [0,1]*/); - showImageInGLView(*bmpObsStereoRight, img_intensity); + showImageInGLView(*bmpObsStereoRight, img_intensity); + } } if (classID->derivedFrom(CLASS_ID(CObservation))) diff --git a/libs/maps/include/mrpt/obs/CObservationRotatingScan.h b/libs/maps/include/mrpt/obs/CObservationRotatingScan.h index 71e3da3cda..248242d943 100644 --- a/libs/maps/include/mrpt/obs/CObservationRotatingScan.h +++ b/libs/maps/include/mrpt/obs/CObservationRotatingScan.h @@ -30,7 +30,9 @@ class CObservationPointCloud; * rotating scanner. This class is the preferred alternative to * CObservationVelodyneScan and CObservation2DRangeScan in MRPT 2.x, since it * exposes range data as an organized matrix, more convenient for feature - * detection directly on "range images". + * detection directly on "range images" and on points stored as a matrix in the + * member organizedPoints. + * * This class can also import data from KITTI dataset-like binary files * containing unorganized (non "undistorted", i.e. without compensation for * lidar motion) point clouds, which get organized into a 2D range image for @@ -85,9 +87,16 @@ class CObservationRotatingScan : public CObservation * (i.e. invalid range). This member must be always provided, containing the * ranges for the STRONGEST ray returns. * To obtain ranges in meters, multiply this matrix by `rangeResolution`. + * + * \sa organizedPoints */ mrpt::math::CMatrix_u16 rangeImage{0, 0}; + /** If present, it contains all 3D points, in local coordinates wrt the + * sensor, for all !=0 entries in \a rangeImage. + */ + mrpt::math::CMatrixDynamic organizedPoints{0, 0}; + /** Optionally, an intensity channel. Matrix with a 0x0 size if not * provided. */ mrpt::math::CMatrix_u8 intensityImage{0, 0}; @@ -109,7 +118,8 @@ class CObservationRotatingScan : public CObservation */ double startAzimuth{-M_PI}, azimuthSpan{2 * M_PI}; - /** Time(in seconds) that passed since `startAzimuth` to* `endAzimuth`. */ + /** Time (in seconds) that passed since `startAzimuth` (first column) to + * `endAzimuth` (last column). */ double sweepDuration{.0}; /** The driver should fill in this observation */ @@ -124,8 +134,6 @@ class CObservationRotatingScan : public CObservation * of reference */ mrpt::poses::CPose3D sensorPose; - // TODO: Calibration!! - /** The local computer-based timestamp based on the * reception of the message in the computer. \sa * has_satellite_timestamp, CObservation::timestamp in the @@ -146,7 +154,6 @@ class CObservationRotatingScan : public CObservation void fromVelodyne(const mrpt::obs::CObservationVelodyneScan& o); void fromScan2D(const mrpt::obs::CObservation2DRangeScan& o); - void fromPointCloud(const mrpt::obs::CObservationPointCloud& o); /** Will convert from another observation if it's any of the supported * source types (see fromVelodyne(), fromScan2D(), fromPointCloud()) and diff --git a/libs/maps/src/obs/CObservationRotatingScan.cpp b/libs/maps/src/obs/CObservationRotatingScan.cpp index 1864601583..6b59ee3a86 100644 --- a/libs/maps/src/obs/CObservationRotatingScan.cpp +++ b/libs/maps/src/obs/CObservationRotatingScan.cpp @@ -24,8 +24,6 @@ using namespace mrpt::obs; // This must be added to any CSerializable class implementation file. IMPLEMENTS_SERIALIZABLE(CObservationRotatingScan, CObservation, mrpt::obs) -// static CSinCosLookUpTableFor2DScans velodyne_sincos_tables; - using RotScan = CObservationRotatingScan; mrpt::system::TTimeStamp RotScan::getOriginalReceivedTimeStamp() const @@ -33,7 +31,7 @@ mrpt::system::TTimeStamp RotScan::getOriginalReceivedTimeStamp() const return originalReceivedTimestamp; } -uint8_t RotScan::serializeGetVersion() const { return 0; } +uint8_t RotScan::serializeGetVersion() const { return 1; } void RotScan::serializeTo(mrpt::serialization::CArchive& out) const { out << timestamp << sensorLabel << rowCount << columnCount @@ -60,6 +58,12 @@ void RotScan::serializeTo(mrpt::serialization::CArchive& out) const ASSERT_EQUAL_(ly.second.rows(), rowCount); out.WriteBufferFixEndianness(&ly.second(0, 0), ly.second.size()); } + + // v1: + out.WriteAs(organizedPoints.cols()); + out.WriteAs(organizedPoints.rows()); + for (const auto& pt : organizedPoints) + out << pt; } void RotScan::serializeFrom(mrpt::serialization::CArchive& in, uint8_t version) @@ -67,6 +71,7 @@ void RotScan::serializeFrom(mrpt::serialization::CArchive& in, uint8_t version) switch (version) { case 0: + case 1: { in >> timestamp >> sensorLabel >> rowCount >> columnCount >> rangeResolution >> startAzimuth >> azimuthSpan >> @@ -100,6 +105,15 @@ void RotScan::serializeFrom(mrpt::serialization::CArchive& in, uint8_t version) im.resize(nRows, nCols); in.ReadBufferFixEndianness(&im(0, 0), im.size()); } + + if (version >= 1) + { // v1 + const auto nIntCols = in.ReadAs(), + nIntRows = in.ReadAs(); + organizedPoints.resize(nIntRows, nIntCols); + for (auto& pt : organizedPoints) + in >> pt; + } } break; default: MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(version); @@ -118,6 +132,8 @@ void RotScan::getDescriptionAsText(std::ostream& o) const o << "lidarModel: " << lidarModel << "\n"; o << "Range rows=" << rowCount << " cols=" << columnCount << "\n"; o << "Range resolution=" << rangeResolution << " [meter]\n"; + o << "Has organized points=" << (!organizedPoints.empty() ? "YES" : "NO") + << "\n"; o << "Scan azimuth: start=" << mrpt::RAD2DEG(startAzimuth) << " span=" << mrpt::RAD2DEG(azimuthSpan) << "\n"; o << "Sweep duration: " << sweepDuration << " [s]\n"; @@ -130,8 +146,6 @@ void RotScan::getDescriptionAsText(std::ostream& o) const << " (UTC)\n"; } -MRPT_TODO("toPointCloud / calibration"); - void RotScan::fromVelodyne(const mrpt::obs::CObservationVelodyneScan& o) { using Velo = mrpt::obs::CObservationVelodyneScan; @@ -338,6 +352,8 @@ void RotScan::fromVelodyne(const mrpt::obs::CObservationVelodyneScan& o) sweepDuration = 1e-6 * (microsecs_last_pkt - microsecs_1st_pkt) + timeBetweenLastTwoBlocks; + MRPT_TODO("populate organizedPoints"); + // Decode model byte: switch (model) { @@ -392,20 +408,13 @@ void RotScan::fromScan2D(const mrpt::obs::CObservation2DRangeScan& o) if (o.hasIntensity()) intensity_out = o.getScanIntensity(i); } + MRPT_TODO("populate organizedPoints"); this->lidarModel = std::string("2D_SCAN_") + this->sensorLabel; MRPT_END } -void RotScan::fromPointCloud(const mrpt::obs::CObservationPointCloud& o) -{ - MRPT_START - MRPT_TODO("fromPointCloud"); - THROW_EXCEPTION("fromPointCloud() not implemented yet"); - MRPT_END -} - bool RotScan::fromGeneric(const mrpt::obs::CObservation& o) { MRPT_START @@ -420,11 +429,6 @@ bool RotScan::fromGeneric(const mrpt::obs::CObservation& o) fromScan2D(*o2D); return true; } - if (auto oPc = dynamic_cast(&o); oPc) - { - fromPointCloud(*oPc); - return true; - } return false; MRPT_END diff --git a/libs/ros2bridge/include/mrpt/ros2bridge/point_cloud2.h b/libs/ros2bridge/include/mrpt/ros2bridge/point_cloud2.h index e56778cf22..29971c7a6f 100644 --- a/libs/ros2bridge/include/mrpt/ros2bridge/point_cloud2.h +++ b/libs/ros2bridge/include/mrpt/ros2bridge/point_cloud2.h @@ -44,13 +44,19 @@ bool fromROS( const sensor_msgs::msg::PointCloud2& msg, mrpt::maps::CPointsMapXYZI& obj); /** Convert sensor_msgs/PointCloud2 -> mrpt::obs::CObservationRotatingScan. - * Requires point cloud fields: x,y,z,intensity,ring + * Requires point cloud fields: x,y,z,ring[,intensity][,time] + * + * If num_azimuth_divisions=0, it will be taken from the point cloud "width" + * field. + * + * Points are supposed to be given in the sensor frame of reference. + * */ bool fromROS( const sensor_msgs::msg::PointCloud2& m, mrpt::obs::CObservationRotatingScan& o, const mrpt::poses::CPose3D& sensorPoseOnRobot, - unsigned int num_azimuth_divisions = 360); + unsigned int num_azimuth_divisions = 0, float max_intensity = 1000.0f); /** Extract a list of fields found in the point cloud. * Typically: {"x","y","z","intensity"} diff --git a/libs/ros2bridge/src/point_cloud2.cpp b/libs/ros2bridge/src/point_cloud2.cpp index f96de27374..c20346ca94 100644 --- a/libs/ros2bridge/src/point_cloud2.cpp +++ b/libs/ros2bridge/src/point_cloud2.cpp @@ -282,10 +282,11 @@ bool mrpt::ros2bridge::fromROS( const sensor_msgs::msg::PointCloud2& msg, mrpt::obs::CObservationRotatingScan& obj, const mrpt::poses::CPose3D& sensorPoseOnRobot, - unsigned int num_azimuth_divisions) + unsigned int num_azimuth_divisions, float max_intensity) { // Copy point data obj.timestamp = mrpt::ros2bridge::fromROS(msg.header.stamp); + obj.originalReceivedTimestamp = obj.timestamp; bool incompatible = false; const sensor_msgs::msg::PointField *x_field = nullptr, *y_field = nullptr, @@ -323,13 +324,33 @@ bool mrpt::ros2bridge::fromROS( ASSERT_NOT_EQUAL_(ring_min, ring_max); obj.rowCount = ring_max - ring_min + 1; + + const bool inputCloudIsOrganized = msg.height != 1; + + if (!num_azimuth_divisions) + { + if (inputCloudIsOrganized) + { + ASSERT_GT_(msg.width, 0U); + num_azimuth_divisions = msg.width; + } + else + { + THROW_EXCEPTION( + "An explicit value for num_azimuth_divisions must be given if " + "the input cloud is not 'organized'"); + } + } + obj.columnCount = num_azimuth_divisions; obj.rangeImage.resize(obj.rowCount, obj.columnCount); obj.rangeImage.fill(0); - // Default unit: 1cm - if (obj.rangeResolution == 0) obj.rangeResolution = 1e-2; + obj.sensorPose = sensorPoseOnRobot; + + // Default unit: 5mm + if (obj.rangeResolution == 0) obj.rangeResolution = 5e-3; if (i_field) { @@ -339,6 +360,11 @@ bool mrpt::ros2bridge::fromROS( else obj.intensityImage.resize(0, 0); + if (inputCloudIsOrganized) + { + obj.organizedPoints.resize(obj.rowCount, obj.columnCount); + } + // If not, memcpy each group of contiguous fields separately for (unsigned int row = 0; row < msg.height; ++row) { @@ -354,15 +380,24 @@ bool mrpt::ros2bridge::fromROS( get_float_from_field(z_field, msg_data, z); get_uint16_from_field(ring_field, msg_data, ring_id); - const mrpt::math::TPoint3D localPt = - sensorPoseOnRobot.inverseComposePoint( - mrpt::math::TPoint3D(x, y, z)); - const double azimuth = std::atan2(localPt.y, localPt.x); + const mrpt::math::TPoint3D localPt = {x, y, z}; + + unsigned int az_idx; + if (inputCloudIsOrganized) + { + // "azimuth index" is just the "column": + az_idx = col; + } + else + { + // Recover "azimuth index" from trigonometry: + const double azimuth = std::atan2(localPt.y, localPt.x); - const auto az_idx = lround( - (num_azimuth_divisions - 1) * (azimuth + M_PI) / (2 * M_PI)); - ASSERT_GE_(az_idx, 0); - ASSERT_LE_(az_idx, num_azimuth_divisions - 1); + az_idx = lround( + (num_azimuth_divisions - 1) * (azimuth + M_PI) / + (2 * M_PI)); + ASSERT_LE_(az_idx, num_azimuth_divisions - 1); + } // Store in matrix form: obj.rangeImage(ring_id, az_idx) = @@ -372,10 +407,14 @@ bool mrpt::ros2bridge::fromROS( { float intensity; get_float_from_field(i_field, msg_data, intensity); - ASSERT_LE_(intensity, 255.0f); - obj.intensityImage(ring_id, az_idx) = lround(intensity); + obj.intensityImage(ring_id, az_idx) = + lround(255 * intensity / max_intensity); } + + if (inputCloudIsOrganized) + obj.organizedPoints(ring_id, az_idx) = localPt; } } + return true; }