Merge branch 'develop'

apps/ReactiveNavigationDemo/reactive_navigator_demoMain.cpp

@@ -1461,7 +1461,8 @@ void reactive_navigator_demoframe::simulateOneStep(double time_step)
 		}
 
 		// TP Robot:
-		gl_rel_robot->setLocation(lfr.infoPerPTG[sel_PTG].TP_Robot);
+		gl_rel_robot->setLocation(
+			mrpt::math::TPoint3D(lfr.infoPerPTG[sel_PTG].TP_Robot));
 
 	}  // end valid PTG selected
 
@@ -1504,10 +1505,7 @@ void reactive_navigator_demoframe::simulateOneStep(double time_step)
 					gl_robot_ptg_prediction->setPose(
 						lfr.rel_pose_PTG_origin_wrt_sense_NOP);
 				}
-				else
-				{
-					gl_robot_ptg_prediction->setPose(CPose3D());
-				}
+				else { gl_robot_ptg_prediction->setPose(CPose3D()); }
 
 				// Overlay a sequence of robot shapes:
 				if (cbDrawShapePath->IsChecked())

apps/rosbag2rawlog/rosbag2rawlog_main.cpp

@@ -179,8 +179,7 @@ void addTfFrameAsKnown(std::string s)
 }
 
 bool findOutSensorPose(
-	mrpt::poses::CPose3D& des, std::string target_frame,
-	std::string source_frame,
+	mrpt::poses::CPose3D& des, std::string referenceFrame, std::string frame,
 	const std::optional<mrpt::poses::CPose3D>& fixedSensorPose)
 {
 	if (fixedSensorPose)
@@ -191,16 +190,16 @@ bool findOutSensorPose(
 
 	// TF1 old frames started with "/foo", forbidden in TF2.
 	// Handle this since this program is for importing old ROS1 bags:
-	removeTrailingSlash(target_frame);
-	removeTrailingSlash(source_frame);
+	removeTrailingSlash(referenceFrame);
+	removeTrailingSlash(frame);
 
 	try
 	{
 		ASSERT_(tfBuffer);
 
 		geometry_msgs::TransformStamped ref_to_trgFrame =
 			tfBuffer->lookupTransform(
-				target_frame, source_frame, {} /*latest value*/);
+				frame, referenceFrame, {} /*latest value*/);
 
 		tf2::Transform tf;
 		tf2::fromMsg(ref_to_trgFrame.transform, tf);
@@ -209,7 +208,7 @@ bool findOutSensorPose(
 #if 0
 		std::cout << mrpt::format(
 			"[findOutSensorPose] Found pose %s -> %s: %s\n",
-			source_frame.c_str(), target_frame.c_str(), des.asString().c_str());
+			referenceFrame.c_str(), frame.c_str(), des.asString().c_str());
 #endif
 
 		return true;

appveyor.yml

@@ -1,5 +1,5 @@
 # version format
-version: 2.12.1-{branch}-build{build}
+version: 2.12.2-{branch}-build{build}
 
 os: Visual Studio 2019
 

doc/requirements.txt

@@ -3,7 +3,7 @@ Babel==2.12.1
 certifi==2023.7.22
 charset-normalizer==3.2.0
 docutils==0.17.1
-idna==3.4
+idna==3.7
 imagesize==1.4.1
 Jinja2==3.1.3
 latexcodec==2.0.1

doc/source/doxygen-docs/changelog.md

@@ -1,5 +1,19 @@
 \page changelog Change Log
 
+# Version 2.12.2: Released April 23rd, 2024
+- Changes in libraries:
+  - \ref mrpt_math_grp:
+    - Remove deprecated headers:
+      - `<mrpt/math/lightweight_geom_data_frwds.h>`
+      - `<mrpt/math/lightweight_geom_data.h>`
+    - Static constructors of mrpt::math::TPoint2D and mrpt::math::TPoint3D marked as `[[nodiscard]]`.
+  - \ref mrpt_opengl_grp:
+    - Render pipeline is now robust against exceptions thrown in the 3D entities boundingBox determination methods.
+- BUG FIXES:
+  - rosbag2rawlog: Fix wrong sensorPose while importing ROS1 datasets.
+  - mrpt::math::TPoint2D::FromVector() and mrpt::math::TPoint3D::FromVector() will silently access undefined memory if an input vector smaller than the vector dimensionality is passed.
+
+
 # Version 2.12.1: Released April 4th, 2024
 - Changes in apps:
   - simul-landmarks: Fix correct generation of sensorLabel and timestamps in observations.

libs/maps/src/opengl/CAngularObservationMesh.cpp

@@ -410,7 +410,7 @@ void CAngularObservationMesh::getUntracedRays(
 {
 	for_each(
 		scanSet.begin(), scanSet.end(),
-		FAddUntracedLines(res, dist, pitchBounds));
+		FAddUntracedLines(res, {dist, .0, .0}, pitchBounds));
 }
 
 TPolygon3D createFromTriangle(const mrpt::opengl::TTriangle& t)

libs/math/include/mrpt/math/TPoint2D.h

@@ -58,7 +58,7 @@ struct TPoint2D_ : public TPoseOrPoint,
 
 	/** Constructor from column vector. */
 	template <typename U>
-	TPoint2D_(const mrpt::math::CMatrixFixed<U, 2, 1>& m)
+	explicit TPoint2D_(const mrpt::math::CMatrixFixed<U, 2, 1>& m)
 	{
 		TPoint2D_data<T>::x = static_cast<T>(m[0]);
 		TPoint2D_data<T>::y = static_cast<T>(m[1]);
@@ -84,11 +84,11 @@ struct TPoint2D_ : public TPoseOrPoint,
 	 * \tparam Vector It can be std::vector<double>, Eigen::VectorXd, etc.
 	 */
 	template <typename Vector>
-	static TPoint2D FromVector(const Vector& v)
+	[[nodiscard]] static TPoint2D_<T> FromVector(const Vector& v)
 	{
 		TPoint2D o;
 		for (int i = 0; i < 2; i++)
-			o[i] = v[i];
+			o[i] = v.at(i);
 		return o;
 	}
 

libs/math/include/mrpt/math/TPoint3D.h

@@ -67,7 +67,7 @@ struct TPoint3D_ : public TPoseOrPoint,
 
 	/** Constructor from column vector. */
 	template <typename U>
-	TPoint3D_(const mrpt::math::CMatrixFixed<U, 3, 1>& m)
+	explicit TPoint3D_(const mrpt::math::CMatrixFixed<U, 3, 1>& m)
 	{
 		TPoint3D_data<T>::x = static_cast<T>(m[0]);
 		TPoint3D_data<T>::y = static_cast<T>(m[1]);
@@ -77,7 +77,7 @@ struct TPoint3D_ : public TPoseOrPoint,
 	/** Implicit constructor from TPoint2D. Zeroes the z.
 	 * \sa TPoint2D
 	 */
-	TPoint3D_(const TPoint2D_<T>& p);
+	explicit TPoint3D_(const TPoint2D_<T>& p);
 	/**
 	 * Constructor from TPose2D, losing information. Zeroes the z.
 	 * \sa TPose2D
@@ -103,11 +103,11 @@ struct TPoint3D_ : public TPoseOrPoint,
 	 * \tparam Vector It can be std::vector<double>, Eigen::VectorXd, etc.
 	 */
 	template <typename Vector>
-	static TPoint3D FromVector(const Vector& v)
+	[[nodiscard]] static TPoint3D_<T> FromVector(const Vector& v)
 	{
 		TPoint3D o;
 		for (int i = 0; i < 3; i++)
-			o[i] = v[i];
+			o[i] = v.at(i);
 		return o;
 	}
 

libs/math/src/TPolygon3D.cpp

@@ -102,7 +102,7 @@ TPolygon3D::TPolygon3D(const TPolygon2D& p) : std::vector<TPoint3D>()
 	size_t N = p.size();
 	resize(N);
 	for (size_t i = 0; i < N; i++)
-		operator[](i) = p[i];
+		operator[](i) = TPoint3D(p[i]);
 }
 void TPolygon3D::createRegularPolygon(
 	size_t numEdges, double radius, TPolygon3D& poly)

libs/opengl/include/mrpt/opengl/TTriangle.h

@@ -57,7 +57,7 @@ struct TTriangle
 		for (size_t i = 0; i < 3; i++)
 		{
 			auto& pp = vertices[i].xyzrgba;
-			pp.pt = p[i];
+			pp.pt = p[i].cast<float>();
 			pp.r = pp.g = pp.b = 0xff;
 		}
 		computeNormals();

libs/opengl/src/CBox.cpp

@@ -162,10 +162,7 @@ void CBox::serializeFrom(mrpt::serialization::CArchive& in, uint8_t version)
 				m_wireframe >> m_lineWidth;
 			// Version 1:
 			if (version >= 1) in >> m_draw_border >> m_solidborder_color;
-			else
-			{
-				m_draw_border = false;
-			}
+			else { m_draw_border = false; }
 			if (version >= 2)
 				CRenderizableShaderTriangles::params_deserialize(in);
 
@@ -201,5 +198,5 @@ bool CBox::traceRay(
 auto CBox::internalBoundingBoxLocal() const -> mrpt::math::TBoundingBoxf
 {
 	return mrpt::math::TBoundingBoxf::FromUnsortedPoints(
-		m_corner_min, m_corner_max);
+		m_corner_min.cast<float>(), m_corner_max.cast<float>());
 }

libs/opengl/src/CGeneralizedEllipsoidTemplate.cpp

@@ -146,6 +146,8 @@ void CGeneralizedEllipsoidTemplate<2>::implUpdate_Triangles()
 template <>
 void CGeneralizedEllipsoidTemplate<3>::implUpdate_Triangles()
 {
+	using P3f = mrpt::math::TPoint3Df;
+
 	const auto& pts = m_render_pts;
 
 	// Render precomputed points in m_render_pts:
@@ -173,9 +175,9 @@ void CGeneralizedEllipsoidTemplate<3>::implUpdate_Triangles()
 
 		tris.emplace_back(
 			// Points
-			pts[0], pts[idxp], pts[idx],
+			P3f(pts[0]), P3f(pts[idxp]), P3f(pts[idx]),
 			// Normals:
-			pts[0], pts[idxp], pts[idx]);
+			P3f(pts[0]), P3f(pts[idxp]), P3f(pts[idx]));
 	}
 
 	// Middle slices: triangle strip (if it were solid)
@@ -192,14 +194,14 @@ void CGeneralizedEllipsoidTemplate<3>::implUpdate_Triangles()
 
 			tris.emplace_back(
 				// Points
-				pts[idx0 + i], pts[idx0 + ii], pts[idx1 + i],
+				P3f(pts[idx0 + i]), P3f(pts[idx0 + ii]), P3f(pts[idx1 + i]),
 				// Normals:
-				pts[idx0 + i], pts[idx0 + ii], pts[idx1 + i]);
+				P3f(pts[idx0 + i]), P3f(pts[idx0 + ii]), P3f(pts[idx1 + i]));
 			tris.emplace_back(
 				// Points
-				pts[idx1 + ii], pts[idx1 + i], pts[idx0 + ii],
+				P3f(pts[idx1 + ii]), P3f(pts[idx1 + i]), P3f(pts[idx0 + ii]),
 				// Normals:
-				pts[idx1 + ii], pts[idx1 + i], pts[idx0 + ii]);
+				P3f(pts[idx1 + ii]), P3f(pts[idx1 + i]), P3f(pts[idx0 + ii]));
 		}
 	}
 
@@ -215,9 +217,9 @@ void CGeneralizedEllipsoidTemplate<3>::implUpdate_Triangles()
 
 		tris.emplace_back(
 			// Points
-			pts[idx], pts[idxp], pts[idxN],
+			P3f(pts[idx]), P3f(pts[idxp]), P3f(pts[idxN]),
 			// Normals
-			pts[idx], pts[idxp], pts[idxN]);
+			P3f(pts[idx]), P3f(pts[idxp]), P3f(pts[idxN]));
 	}
 
 	// All faces, all vertices, same color:

libs/opengl/src/RenderQueue.cpp

@@ -101,7 +101,17 @@ std::tuple<double, bool, bool> mrpt::opengl::depthAndVisibleInView(
 		mrpt::system::CTimeLoggerEntry tle2(
 			opengl_profiler(), "depthAndVisibleInView.bbox");
 	#endif*/
-	const auto bbox = obj->getBoundingBoxLocalf();
+
+	mrpt::math::TBoundingBoxf bbox;
+	try
+	{
+		bbox = obj->getBoundingBoxLocalf();
+	}
+	catch (const std::exception&)
+	{
+		// use default (0,0,0) bbox
+	}
+
 	/*#ifdef MRPT_OPENGL_PROFILER
 		tle2.stop();
 	#endif*/
@@ -115,25 +125,26 @@ std::tuple<double, bool, bool> mrpt::opengl::depthAndVisibleInView(
 
 	const auto lambdaProcessSample =
 		[&anyVisible, &allVisible, &anyGoodDepth, &quadrants](
-			const mrpt::math::TPoint2Df& uv, const float sampleDepth) {
-			// Do not check for "bboxDepth < 1.0f" since that may only mean
-			// the object is still visible, but farther away than the
-			// farPlane:
-			const bool goodDepth = sampleDepth > -1.0f;
-			anyGoodDepth = anyGoodDepth | goodDepth;
-
-			const bool inside = goodDepth &&
-				(uv.x >= -1 && uv.x <= 1 && uv.y >= -1 && uv.y < 1);
-
-			if (inside) anyVisible = true;
-			else
-				allVisible = false;
-
-			// quadrants:
-			int qx = uv.x < -1 ? 0 : (uv.x > 1 ? 2 : 1);
-			int qy = uv.y < -1 ? 0 : (uv.y > 1 ? 2 : 1);
-			quadrants[qx + 3 * qy] = true;
-		};
+			const mrpt::math::TPoint2Df& uv, const float sampleDepth)
+	{
+		// Do not check for "bboxDepth < 1.0f" since that may only mean
+		// the object is still visible, but farther away than the
+		// farPlane:
+		const bool goodDepth = sampleDepth > -1.0f;
+		anyGoodDepth = anyGoodDepth | goodDepth;
+
+		const bool inside =
+			goodDepth && (uv.x >= -1 && uv.x <= 1 && uv.y >= -1 && uv.y < 1);
+
+		if (inside) anyVisible = true;
+		else
+			allVisible = false;
+
+		// quadrants:
+		int qx = uv.x < -1 ? 0 : (uv.x > 1 ? 2 : 1);
+		int qy = uv.y < -1 ? 0 : (uv.y > 1 ? 2 : 1);
+		quadrants[qx + 3 * qy] = true;
+	};
 
 	// 1st) Process the local-representative-point (~body center) sample:
 	// -----------------------------------------------------------------------

libs/poses/include/mrpt/poses/CPoint2D.h

@@ -39,8 +39,10 @@ class CPoint2D : public CPoint<CPoint2D, 2>,
 	mrpt::math::CVectorFixedDouble<2> m_coords;
 
    public:
+	CPoint2D() : CPoint2D(.0, .0) {}
+
 	/** Constructor for initializing point coordinates. */
-	inline CPoint2D(double x = 0, double y = 0)
+	CPoint2D(double x, double y)
 	{
 		m_coords[0] = x;
 		m_coords[1] = y;
@@ -54,7 +56,7 @@ class CPoint2D : public CPoint<CPoint2D, 2>,
 		m_coords[1] = b.y();
 	}
 
-	/** Implicit constructor from lightweight type. */
+	/** Explicit constructor from lightweight type. */
 	explicit CPoint2D(const mrpt::math::TPoint2D& o);
 
 	/** Explicit constructor from lightweight type (loses the z coord). */

libs/poses/include/mrpt/poses/CPoint2DPDFGaussian.h

@@ -26,7 +26,7 @@ class CPoint2DPDFGaussian : public CPoint2DPDF
 	/** Default constructor */
 	CPoint2DPDFGaussian();
 	/** Constructor */
-	CPoint2DPDFGaussian(const CPoint2D& init_Mean);
+	explicit CPoint2DPDFGaussian(const CPoint2D& init_Mean);
 	/** Constructor */
 	CPoint2DPDFGaussian(
 		const CPoint2D& init_Mean, const mrpt::math::CMatrixDouble22& init_Cov);