Progress finishing CObservationRotatingScan

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<float>() *
+					(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<float>() *
+					(1.0f / maxActualInt),
+				true /*already in [0,1]*/);
 
-		showImageInGLView(*bmpObsStereoRight, img_intensity);
+			showImageInGLView(*bmpObsStereoRight, img_intensity);
+		}
 	}
 
 	if (classID->derivedFrom(CLASS_ID(CObservation)))

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<mrpt::math::TPoint3Df> 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

libs/maps/src/obs/CObservationRotatingScan.cpp

@@ -24,16 +24,14 @@ 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
 {
 	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,13 +58,20 @@ 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<uint16_t>(organizedPoints.cols());
+	out.WriteAs<uint16_t>(organizedPoints.rows());
+	for (const auto& pt : organizedPoints)
+		out << pt;
 }
 
 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<uint16_t>(),
+						   nIntRows = in.ReadAs<uint16_t>();
+				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<const CObservationPointCloud*>(&o); oPc)
-	{
-		fromPointCloud(*oPc);
-		return true;
-	}
 	return false;
 
 	MRPT_END

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"}

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;
 }