updated nromal example

tools/CMakeLists.txt

@@ -17,6 +17,7 @@ ADD_SUBDIRECTORY( Info )
 ADD_SUBDIRECTORY( CleanupLocalGrids )
 ADD_SUBDIRECTORY( GlobalBundleAdjustment )
 ADD_SUBDIRECTORY( ReduceGraph )
+ADD_SUBDIRECTORY( LidarCameraCalibration )
 
 IF(OPENCV_NONFREE_FOUND)
 ADD_SUBDIRECTORY( VocabularyComparison )

tools/LidarCameraCalibration/CMakeLists.txt

@@ -0,0 +1,33 @@
+
+ADD_EXECUTABLE(lidarCameraCalibration
+  main.cpp
+  CalibrationProblem.cpp
+  CorrectionSolver.cpp
+  PatternSearchSolver.cpp
+  DownhillSimplexSolver.cpp
+  G2oSolver.cpp
+  GtsamSolver.cpp)
+  
+TARGET_LINK_LIBRARIES(lidarCameraCalibration rtabmap_core)
+
+# For the g2o solver: rtabmap_core links g2o privately.
+IF(G2O_FOUND)
+  IF(g2o_FOUND)
+    TARGET_LINK_LIBRARIES(lidarCameraCalibration g2o::core g2o::solver_eigen)
+  ELSE()
+    TARGET_INCLUDE_DIRECTORIES(lidarCameraCalibration PRIVATE ${G2O_INCLUDE_DIRS})
+    TARGET_LINK_LIBRARIES(lidarCameraCalibration ${G2O_LIBRARIES})
+  ENDIF()
+ENDIF(G2O_FOUND)
+
+# For the GTSAM solver: rtabmap_core links GTSAM privately.
+IF(GTSAM_FOUND)
+  TARGET_LINK_LIBRARIES(lidarCameraCalibration gtsam)
+ENDIF(GTSAM_FOUND)
+
+SET_TARGET_PROPERTIES( lidarCameraCalibration 
+  PROPERTIES OUTPUT_NAME ${PROJECT_PREFIX}-lidarCameraCalibration)
+
+INSTALL(TARGETS lidarCameraCalibration
+		RUNTIME DESTINATION "${CMAKE_INSTALL_BINDIR}" COMPONENT runtime
+		BUNDLE DESTINATION "${CMAKE_BUNDLE_LOCATION}" COMPONENT runtime)

tools/LidarCameraCalibration/CalibrationProblem.cpp

@@ -0,0 +1,130 @@
+/*
+Copyright (c) 2010-2026, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+    * Redistributions of source code must retain the above copyright
+      notice, this list of conditions and the following disclaimer.
+    * Redistributions in binary form must reproduce the above copyright
+      notice, this list of conditions and the following disclaimer in the
+      documentation and/or other materials provided with the distribution.
+    * Neither the name of the Universite de Sherbrooke nor the
+      names of its contributors may be used to endorse or promote products
+      derived from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
+DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+#include "CalibrationProblem.h"
+
+#include <rtabmap/core/util3d_transforms.h>
+
+#include <algorithm>
+#include <cmath>
+
+using namespace rtabmap;
+
+Transform correctionFrom(const double p[6])
+{
+	return Transform(p[0], p[1], p[2], p[3]*M_PI/180.0, p[4]*M_PI/180.0, p[5]*M_PI/180.0);
+}
+
+Eigen::Isometry3d correctionFromDouble(const double p[6])
+{
+	// As Transform(x, y, z, roll, pitch, yaw): yaw * pitch * roll.
+	Eigen::Isometry3d C = Eigen::Isometry3d::Identity();
+	C.translation() = Eigen::Vector3d(p[0], p[1], p[2]);
+	C.linear() = (Eigen::AngleAxisd(p[5]*M_PI/180.0, Eigen::Vector3d::UnitZ()) *
+			Eigen::AngleAxisd(p[4]*M_PI/180.0, Eigen::Vector3d::UnitY()) *
+			Eigen::AngleAxisd(p[3]*M_PI/180.0, Eigen::Vector3d::UnitX())).toRotationMatrix();
+	return C;
+}
+
+CalibrationProblem::CalibrationProblem(const std::vector<Frame> & frames, const std::vector<int> & nodes, float minDepth) :
+	frames_(frames), nodes_(nodes), minDepth_(minDepth), evaluations_(0)
+{
+}
+
+void CalibrationProblem::project(const Transform & C,
+		const std::function<void(const Frame &, size_t, float, float)> & visit) const
+{
+	for(int k : nodes_)
+	{
+		const Frame & f = frames_[k];
+		const Transform scanInCam = (f.scanToCam * C).inverse();
+		const double fx = f.K.at<double>(0,0), fy = f.K.at<double>(1,1);
+		const double cx = f.K.at<double>(0,2), cy = f.K.at<double>(1,2);
+		for(size_t i = 0; i < f.edgePoints.size(); ++i)
+		{
+			const cv::Point3f pc = util3d::transformPoint(f.edgePoints[i], scanInCam);
+			if(pc.z < minDepth_)
+			{
+				continue;
+			}
+			const float u = fx * pc.x / pc.z + cx, v = fy * pc.y / pc.z + cy;
+			if(u < 0 || v < 0 || u >= f.gray.cols - 1 || v >= f.gray.rows - 1)
+			{
+				continue;
+			}
+			visit(f, i, u, v);
+		}
+	}
+}
+
+double CalibrationProblem::score(const Transform & C) const
+{
+	++evaluations_;
+	double sum = 0.0;
+	project(C, [&](const Frame & f, size_t i, float u, float v) {
+		const int u0 = int(u), v0 = int(v);
+		const float a = u - u0, b = v - v0;
+		const float s =
+				(1-a)*(1-b)*f.edgeScore.at<float>(v0, u0) + a*(1-b)*f.edgeScore.at<float>(v0, u0+1) +
+				(1-a)*b*f.edgeScore.at<float>(v0+1, u0) + a*b*f.edgeScore.at<float>(v0+1, u0+1);
+		sum += f.edgeWeights[i] * s;
+	});
+	// The frames' edge points are selected again between passes: not cached.
+	double weights = 0.0;
+	for(int k : nodes_)
+	{
+		for(float w : frames_[k].edgeWeights)
+		{
+			weights += w;
+		}
+	}
+	return weights > 0.0 ? sum / weights : 0.0;
+}
+
+double CalibrationProblem::edgeDistance(const Frame & f, size_t i, const double p[6], double cap) const
+{
+	const cv::Point3f & pt = f.edgePoints[i];
+	const Eigen::Vector3d pc =
+			(f.scanToCam.toEigen3d() * correctionFromDouble(p)).inverse() * Eigen::Vector3d(pt.x, pt.y, pt.z);
+	if(pc.z() < minDepth_)
+	{
+		return cap;
+	}
+	const double u = f.K.at<double>(0,0) * pc.x() / pc.z() + f.K.at<double>(0,2);
+	const double v = f.K.at<double>(1,1) * pc.y() / pc.z() + f.K.at<double>(1,2);
+	if(u < 0 || v < 0 || u >= f.edgeDistance.cols - 1 || v >= f.edgeDistance.rows - 1)
+	{
+		return cap;
+	}
+	const int u0 = int(u), v0 = int(v);
+	const double a = u - u0, b = v - v0;
+	const cv::Mat & d = f.edgeDistance;
+	const double distance =
+			(1-a)*(1-b)*d.at<float>(v0, u0) + a*(1-b)*d.at<float>(v0, u0+1) +
+			(1-a)*b*d.at<float>(v0+1, u0) + a*b*d.at<float>(v0+1, u0+1);
+	return std::min(distance, cap);
+}

tools/LidarCameraCalibration/CalibrationProblem.h

@@ -0,0 +1,113 @@
+/*
+Copyright (c) 2010-2026, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+    * Redistributions of source code must retain the above copyright
+      notice, this list of conditions and the following disclaimer.
+    * Redistributions in binary form must reproduce the above copyright
+      notice, this list of conditions and the following disclaimer in the
+      documentation and/or other materials provided with the distribution.
+    * Neither the name of the Universite de Sherbrooke nor the
+      names of its contributors may be used to endorse or promote products
+      derived from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
+DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+#ifndef LIDARCAMERACALIBRATION_CALIBRATIONPROBLEM_H_
+#define LIDARCAMERACALIBRATION_CALIBRATIONPROBLEM_H_
+
+#include <rtabmap/core/Transform.h>
+
+#include <opencv2/core/core.hpp>
+#include <Eigen/Geometry>
+
+#include <functional>
+#include <vector>
+
+// What makes a lidar point an edge point, in order of precedence.
+enum EdgeType
+{
+	kEdgeDepth = 0,      // in front of a depth discontinuity
+	kEdgeCrease = 1,     // where the surface's orientation changes (e.g., wall and floor)
+	kEdgeIntensity = 2   // where the surface's reflectance changes
+};
+
+// One node of the database: its image, its lidar scan, and the lidar's edges.
+struct Frame
+{
+	int id;
+	cv::Mat gray;          // as stored, matching the camera model
+	cv::Mat edges;         // CV_8U: the image's edges (Canny), non-zero on an edge
+	cv::Mat edgeDistance;  // CV_32F: distance (pixels) to the image's nearest edge
+	cv::Mat edgeScore;     // CV_32F: 1 on the image's edges, decaying with the distance to them
+	cv::Mat K;             // CV_64F 3x3
+	rtabmap::Transform scanToCam;  // camera pose in the scan frame, before correction
+	cv::Mat cloud;         // Nx4 CV_32F, scan frame: x y z intensity (0 without intensity)
+	cv::Mat normals;       // Mx6 CV_32F, scan frame: voxelized points and their normals (for creases)
+	bool hasIntensity;
+	// The robot's speeds (m/s, deg/s; negative if unknown): instantaneous, odometry's when the
+	// node was added, and mean along odometry over the meanWindow (s) since the previous
+	// node, which is when an assembled scan was taken.
+	float linearSpeed = -1.0f, angularSpeed = -1.0f;
+	float meanLinearSpeed = -1.0f, meanAngularSpeed = -1.0f, meanWindow = 0.0f;
+	int meanPoses = 0;  // odometry steps the mean speed is from: more than 1 with intermediate nodes
+	std::vector<cv::Point3f> edgePoints;  // depth and intensity edge points, scan frame
+	std::vector<float> edgeWeights;
+	std::vector<unsigned char> edgeTypes; // EdgeType of each edge point
+};
+
+// The correction of parameters p = tx ty tz (m), rx ry rz (deg), in the camera frame.
+rtabmap::Transform correctionFrom(const double p[6]);
+// The same, in double precision: rtabmap's Transform is float, too coarse for the small
+// steps of numerical derivatives.
+Eigen::Isometry3d correctionFromDouble(const double p[6]);
+
+// The lidar edge points of some nodes, and how well a candidate correction C lines them
+// up with their images' edges. This is all a solver sees of the calibration.
+class CalibrationProblem
+{
+public:
+	// The frames' edge points are read at each call, so they can be selected again after
+	// the problem is made. minDepth: lidar points closer than this to the camera (m) are
+	// ignored.
+	CalibrationProblem(const std::vector<Frame> & frames, const std::vector<int> & nodes, float minDepth);
+
+	const std::vector<Frame> & frames() const {return frames_;}
+	const std::vector<int> & nodes() const {return nodes_;}
+	float minDepth() const {return minDepth_;}
+
+	// Calls visit(frame, point index, u, v) for each edge point that C projects in its
+	// image, at full resolution.
+	void project(const rtabmap::Transform & C,
+			const std::function<void(const Frame &, size_t, float, float)> & visit) const;
+
+	// What the direct solvers maximize: the weighted mean of the image edges' score under
+	// the projected lidar edge points, in [0, 1]. A point out of its image counts as 0.
+	double score(const rtabmap::Transform & C) const;
+	int evaluations() const {return evaluations_;}
+
+	// What the least-squares solvers minimize, per point: the distance (pixels) from
+	// where p projects edge point i of f to f's nearest image edge, capped; the cap where
+	// it does not project in the image. In double precision, for numerical derivatives.
+	double edgeDistance(const Frame & f, size_t i, const double p[6], double cap) const;
+
+private:
+	const std::vector<Frame> & frames_;
+	std::vector<int> nodes_;
+	float minDepth_;
+	mutable int evaluations_;
+};
+
+#endif /* LIDARCAMERACALIBRATION_CALIBRATIONPROBLEM_H_ */

tools/LidarCameraCalibration/CorrectionSolver.cpp

@@ -0,0 +1,74 @@
+/*
+Copyright (c) 2010-2026, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+    * Redistributions of source code must retain the above copyright
+      notice, this list of conditions and the following disclaimer.
+    * Redistributions in binary form must reproduce the above copyright
+      notice, this list of conditions and the following disclaimer in the
+      documentation and/or other materials provided with the distribution.
+    * Neither the name of the Universite de Sherbrooke nor the
+      names of its contributors may be used to endorse or promote products
+      derived from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
+DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+#include "CorrectionSolver.h"
+#include "DownhillSimplexSolver.h"
+#include "G2oSolver.h"
+#include "GtsamSolver.h"
+#include "PatternSearchSolver.h"
+
+#include <rtabmap/core/Version.h>
+
+const double kInitialSteps[6] = {0.04, 0.04, 0.04, 2.0, 2.0, 2.0};
+
+std::unique_ptr<CorrectionSolver> createSolver(const std::string & name, float sigma)
+{
+	(void)sigma;  // used only by the least-squares solvers
+	if(name == "pattern")
+	{
+		return std::unique_ptr<CorrectionSolver>(new PatternSearchSolver());
+	}
+	if(name == "simplex")
+	{
+		return std::unique_ptr<CorrectionSolver>(new DownhillSimplexSolver());
+	}
+#ifdef RTABMAP_G2O
+	if(name == "g2o")
+	{
+		return std::unique_ptr<CorrectionSolver>(new G2oSolver(sigma));
+	}
+#endif
+#ifdef RTABMAP_GTSAM
+	if(name == "gtsam")
+	{
+		return std::unique_ptr<CorrectionSolver>(new GtsamSolver(sigma));
+	}
+#endif
+	return std::unique_ptr<CorrectionSolver>();
+}
+
+std::vector<std::string> availableSolvers()
+{
+	std::vector<std::string> names = {"simplex", "pattern"};
+#ifdef RTABMAP_G2O
+	names.push_back("g2o");
+#endif
+#ifdef RTABMAP_GTSAM
+	names.push_back("gtsam");
+#endif
+	return names;
+}

tools/LidarCameraCalibration/CorrectionSolver.h

@@ -0,0 +1,58 @@
+/*
+Copyright (c) 2010-2026, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+    * Redistributions of source code must retain the above copyright
+      notice, this list of conditions and the following disclaimer.
+    * Redistributions in binary form must reproduce the above copyright
+      notice, this list of conditions and the following disclaimer in the
+      documentation and/or other materials provided with the distribution.
+    * Neither the name of the Universite de Sherbrooke nor the
+      names of its contributors may be used to endorse or promote products
+      derived from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
+DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+#ifndef LIDARCAMERACALIBRATION_CORRECTIONSOLVER_H_
+#define LIDARCAMERACALIBRATION_CORRECTIONSOLVER_H_
+
+#include "CalibrationProblem.h"
+
+#include <memory>
+#include <string>
+#include <vector>
+
+// Parameters: p = tx ty tz (m), rx ry rz (deg), in the camera frame (see correctionFrom()).
+// Initial search steps, also the scale of each parameter.
+extern const double kInitialSteps[6];
+
+// Finds the correction best aligning the problem's lidar edges with its images' edges.
+// A new solver implements this and is added to createSolver().
+class CorrectionSolver
+{
+public:
+	virtual ~CorrectionSolver() {}
+	virtual const char * name() const = 0;
+	// Improves p from its value. Without estimateTranslation, p's translation is left as is.
+	virtual void solve(const CalibrationProblem & problem, bool estimateTranslation, double p[6]) const = 0;
+};
+
+// The solver named "pattern", "simplex", "g2o" or "gtsam" (the last two if rtabmap was
+// built with them), or null. sigma: the image edges' fall off (pixels).
+std::unique_ptr<CorrectionSolver> createSolver(const std::string & name, float sigma);
+// The names createSolver() knows in this build.
+std::vector<std::string> availableSolvers();
+
+#endif /* LIDARCAMERACALIBRATION_CORRECTIONSOLVER_H_ */

tools/LidarCameraCalibration/DownhillSimplexSolver.cpp

@@ -0,0 +1,95 @@
+/*
+Copyright (c) 2010-2026, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+    * Redistributions of source code must retain the above copyright
+      notice, this list of conditions and the following disclaimer.
+    * Redistributions in binary form must reproduce the above copyright
+      notice, this list of conditions and the following disclaimer in the
+      documentation and/or other materials provided with the distribution.
+    * Neither the name of the Universite de Sherbrooke nor the
+      names of its contributors may be used to endorse or promote products
+      derived from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
+DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+#include "DownhillSimplexSolver.h"
+
+#include <opencv2/core/optim.hpp>
+
+#include <algorithm>
+#include <vector>
+
+namespace {
+
+// The score to minimize, as a function of the solved parameters x; the others stay as
+// in p.
+class NegativeScore : public cv::MinProblemSolver::Function
+{
+public:
+	NegativeScore(const CalibrationProblem & problem, const std::vector<int> & solved, const double p[6]) :
+		problem_(problem), solved_(solved)
+	{
+		std::copy(p, p + 6, p_);
+	}
+	int getDims() const override {return (int)solved_.size();}
+	double calc(const double * x) const override
+	{
+		double q[6];
+		std::copy(p_, p_ + 6, q);
+		for(size_t i = 0; i < solved_.size(); ++i)
+		{
+			q[solved_[i]] = x[i];
+		}
+		return -problem_.score(correctionFrom(q));
+	}
+private:
+	const CalibrationProblem & problem_;
+	std::vector<int> solved_;
+	double p_[6];
+};
+
+}  // namespace
+
+void DownhillSimplexSolver::solve(const CalibrationProblem & problem, bool estimateTranslation, double p[6]) const
+{
+	std::vector<int> solved;
+	for(int k = estimateTranslation ? 0 : 3; k < 6; ++k)
+	{
+		solved.push_back(k);
+	}
+
+	cv::Ptr<cv::DownhillSolver> solver = cv::DownhillSolver::create(
+			cv::makePtr<NegativeScore>(problem, solved, p),
+			cv::noArray(),
+			cv::TermCriteria(cv::TermCriteria::MAX_ITER + cv::TermCriteria::EPS, 5000, 1e-9));
+	cv::Mat x(1, (int)solved.size(), CV_64F), step(1, (int)solved.size(), CV_64F);
+	for(size_t i = 0; i < solved.size(); ++i)
+	{
+		x.at<double>(0, i) = p[solved[i]];
+		step.at<double>(0, i) = kInitialSteps[solved[i]];
+	}
+	// Started again from its result: a simplex can shrink before it reaches the
+	// optimum, the restart gives it its full size back.
+	for(int restart = 0; restart < 2; ++restart)
+	{
+		solver->setInitStep(step);
+		solver->minimize(x);
+	}
+	for(size_t i = 0; i < solved.size(); ++i)
+	{
+		p[solved[i]] = x.at<double>(0, i);
+	}
+}

tools/LidarCameraCalibration/DownhillSimplexSolver.h

@@ -0,0 +1,42 @@
+/*
+Copyright (c) 2010-2026, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+    * Redistributions of source code must retain the above copyright
+      notice, this list of conditions and the following disclaimer.
+    * Redistributions in binary form must reproduce the above copyright
+      notice, this list of conditions and the following disclaimer in the
+      documentation and/or other materials provided with the distribution.
+    * Neither the name of the Universite de Sherbrooke nor the
+      names of its contributors may be used to endorse or promote products
+      derived from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
+DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+#ifndef LIDARCAMERACALIBRATION_DOWNHILLSIMPLEXSOLVER_H_
+#define LIDARCAMERACALIBRATION_DOWNHILLSIMPLEXSOLVER_H_
+
+#include "CorrectionSolver.h"
+
+// OpenCV's Nelder-Mead simplex: moves all the parameters together, so it follows
+// directions in which they are coupled.
+class DownhillSimplexSolver : public CorrectionSolver
+{
+public:
+	const char * name() const override {return "simplex";}
+	void solve(const CalibrationProblem & problem, bool estimateTranslation, double p[6]) const override;
+};
+
+#endif /* LIDARCAMERACALIBRATION_DOWNHILLSIMPLEXSOLVER_H_ */

tools/LidarCameraCalibration/G2oSolver.cpp

@@ -0,0 +1,188 @@
+/*
+Copyright (c) 2010-2026, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+    * Redistributions of source code must retain the above copyright
+      notice, this list of conditions and the following disclaimer.
+    * Redistributions in binary form must reproduce the above copyright
+      notice, this list of conditions and the following disclaimer in the
+      documentation and/or other materials provided with the distribution.
+    * Neither the name of the Universite de Sherbrooke nor the
+      names of its contributors may be used to endorse or promote products
+      derived from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
+DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+#include "G2oSolver.h"
+
+#ifdef RTABMAP_G2O
+
+#include <g2o/core/base_unary_edge.h>
+#include <g2o/core/base_vertex.h>
+#include <g2o/core/block_solver.h>
+#include <g2o/core/optimization_algorithm_levenberg.h>
+#include <g2o/core/robust_kernel_impl.h>
+#include <g2o/core/sparse_optimizer.h>
+#include <g2o/solvers/eigen/linear_solver_eigen.h>
+
+#include <algorithm>
+
+namespace {
+
+// The solved parameters of p (the rotation, and the translation if estimated).
+template<int D>
+class VertexCorrection : public g2o::BaseVertex<D, Eigen::Matrix<double, D, 1> >
+{
+public:
+	EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+	void setToOriginImpl() override {this->_estimate.setZero();}
+	void oplusImpl(const double * update) override  // as rtabmap's own g2o vertices
+	{
+		for(int i = 0; i < D; ++i)
+		{
+			this->_estimate[i] += update[i];
+		}
+	}
+	bool read(std::istream &) override {return false;}
+	bool write(std::ostream &) const override {return true;}
+};
+
+// One lidar edge point: CalibrationProblem::edgeDistance() for it.
+template<int D>
+class EdgeToImageEdge : public g2o::BaseUnaryEdge<1, double, VertexCorrection<D> >
+{
+public:
+	EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+	EdgeToImageEdge(const CalibrationProblem & problem, const Frame & frame, size_t point,
+			const std::vector<int> & solved, const double p[6], double cap) :
+		problem_(problem), frame_(frame), point_(point), solved_(solved), cap_(cap)
+	{
+		std::copy(p, p + 6, p_);
+	}
+
+	void computeError() override
+	{
+		this->_error[0] = residual(static_cast<const VertexCorrection<D> *>(this->_vertices[0])->estimate());
+	}
+
+	// Central differences, with steps of the precision the projection needs rather than
+	// g2o's default (1e-9), lost in the image's distance map.
+	void linearizeOplus() override
+	{
+		const Eigen::Matrix<double, D, 1> x = static_cast<const VertexCorrection<D> *>(this->_vertices[0])->estimate();
+		for(int k = 0; k < D; ++k)
+		{
+			const double h = solved_[k] < 3 ? 1e-4 : 1e-3;  // m, deg
+			Eigen::Matrix<double, D, 1> plus = x, minus = x;
+			plus[k] += h;
+			minus[k] -= h;
+			this->_jacobianOplusXi(0, k) = (residual(plus) - residual(minus)) / (2.0 * h);
+		}
+	}
+
+	bool read(std::istream &) override {return false;}
+	bool write(std::ostream &) const override {return true;}
+
+private:
+	double residual(const Eigen::Matrix<double, D, 1> & x) const
+	{
+		double q[6];
+		std::copy(p_, p_ + 6, q);
+		for(int k = 0; k < D; ++k)
+		{
+			q[solved_[k]] = x[k];
+		}
+		return problem_.edgeDistance(frame_, point_, q, cap_);
+	}
+
+	const CalibrationProblem & problem_;
+	const Frame & frame_;
+	size_t point_;
+	std::vector<int> solved_;
+	double p_[6];
+	double cap_;
+};
+
+}  // namespace
+
+void G2oSolver::solve(const CalibrationProblem & problem, bool estimateTranslation, double p[6]) const
+{
+	coarse_.solve(problem, estimateTranslation, p);
+	if(estimateTranslation)
+	{
+		solveWith<6>(problem, {0, 1, 2, 3, 4, 5}, p);
+	}
+	else
+	{
+		solveWith<3>(problem, {3, 4, 5}, p);
+	}
+}
+
+template<int D>
+void G2oSolver::solveWith(const CalibrationProblem & problem, const std::vector<int> & solved, double p[6]) const
+{
+	typedef g2o::BlockSolver<g2o::BlockSolverTraits<D, 1> > Block;
+	typedef g2o::LinearSolverEigen<typename Block::PoseMatrixType> Linear;
+
+	Eigen::Matrix<double, D, 1> x;
+	for(int k = 0; k < D; ++k)
+	{
+		x[k] = p[solved[k]];
+	}
+	for(double scale : {9.0, 3.0, 1.0})
+	{
+		const double delta = scale * sigma_;
+		g2o::SparseOptimizer optimizer;
+#ifdef RTABMAP_G2O_CPP11
+		optimizer.setAlgorithm(new g2o::OptimizationAlgorithmLevenberg(
+				std::unique_ptr<Block>(new Block(std::unique_ptr<Linear>(new Linear())))));
+#else
+		optimizer.setAlgorithm(new g2o::OptimizationAlgorithmLevenberg(new Block(new Linear())));
+#endif
+		VertexCorrection<D> * vertex = new VertexCorrection<D>();
+		vertex->setEstimate(x);
+		vertex->setId(0);
+		optimizer.addVertex(vertex);
+
+		// Beyond a few times the kernel's scale, a point does not count anyway.
+		const double cap = 10.0 * delta;
+		int id = 1;
+		for(int k : problem.nodes())
+		{
+			const Frame & f = problem.frames()[k];
+			for(size_t i = 0; i < f.edgePoints.size(); ++i)
+			{
+				EdgeToImageEdge<D> * edge = new EdgeToImageEdge<D>(problem, f, i, solved, p, cap);
+				edge->setId(id++);
+				edge->setVertex(0, vertex);
+				edge->setMeasurement(0.0);
+				edge->setInformation(Eigen::Matrix<double, 1, 1>::Constant(f.edgeWeights[i]));
+				g2o::RobustKernelWelsch * kernel = new g2o::RobustKernelWelsch();
+				kernel->setDelta(delta);
+				edge->setRobustKernel(kernel);
+				optimizer.addEdge(edge);
+			}
+		}
+		optimizer.initializeOptimization();
+		optimizer.optimize(10);  // close already, after the coarse search
+		x = vertex->estimate();
+	}
+	for(int k = 0; k < D; ++k)
+	{
+		p[solved[k]] = x[k];
+	}
+}
+
+#endif

tools/LidarCameraCalibration/G2oSolver.h

@@ -0,0 +1,65 @@
+/*
+Copyright (c) 2010-2026, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+    * Redistributions of source code must retain the above copyright
+      notice, this list of conditions and the following disclaimer.
+    * Redistributions in binary form must reproduce the above copyright
+      notice, this list of conditions and the following disclaimer in the
+      documentation and/or other materials provided with the distribution.
+    * Neither the name of the Universite de Sherbrooke nor the
+      names of its contributors may be used to endorse or promote products
+      derived from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
+DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+#ifndef LIDARCAMERACALIBRATION_G2OSOLVER_H_
+#define LIDARCAMERACALIBRATION_G2OSOLVER_H_
+
+#include "CorrectionSolver.h"
+#include "PatternSearchSolver.h"
+
+#include <rtabmap/core/Version.h>
+
+#ifdef RTABMAP_G2O
+
+// Least squares with g2o: Levenberg-Marquardt on the distances of the lidar edge points
+// to the images' edges (chamfer matching), weighted by the points' weights.
+//
+// - A redescending robust kernel (Welsch) makes a point far from any image edge count for
+//   nothing, as the score does: many lidar edges have no counterpart in the image
+//   (speckle, surfaces the camera does not see the same way).
+// - Levenberg-Marquardt only follows the local slope, and each point is pulled toward its
+//   nearest image edge, often not its own when far from the solution: from a few degrees
+//   away, it stops in a local minimum. So a coarse pattern search gets close first, then
+//   the kernel's scale goes from wide to narrow (9, 3, then 1 x sigma).
+class G2oSolver : public CorrectionSolver
+{
+public:
+	G2oSolver(double sigma) : sigma_(sigma), coarse_(4) {}
+	const char * name() const override {return "g2o";}
+	void solve(const CalibrationProblem & problem, bool estimateTranslation, double p[6]) const override;
+
+private:
+	template<int D>
+	void solveWith(const CalibrationProblem & problem, const std::vector<int> & solved, double p[6]) const;
+
+	double sigma_;
+	PatternSearchSolver coarse_;  // steps of 2 down to 0.25 deg
+};
+
+#endif
+
+#endif /* LIDARCAMERACALIBRATION_G2OSOLVER_H_ */

tools/LidarCameraCalibration/GtsamSolver.cpp

@@ -0,0 +1,159 @@
+/*
+Copyright (c) 2010-2026, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+    * Redistributions of source code must retain the above copyright
+      notice, this list of conditions and the following disclaimer.
+    * Redistributions in binary form must reproduce the above copyright
+      notice, this list of conditions and the following disclaimer in the
+      documentation and/or other materials provided with the distribution.
+    * Neither the name of the Universite de Sherbrooke nor the
+      names of its contributors may be used to endorse or promote products
+      derived from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
+DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+#include "GtsamSolver.h"
+
+#ifdef RTABMAP_GTSAM
+
+#include <gtsam/linear/NoiseModel.h>
+#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
+#include <gtsam/nonlinear/NonlinearFactor.h>
+#include <gtsam/nonlinear/NonlinearFactorGraph.h>
+#include <gtsam/nonlinear/Values.h>
+
+#include <algorithm>
+#include <cmath>
+
+namespace {
+
+// One lidar edge point: CalibrationProblem::edgeDistance() for it, as a function of the
+// solved parameters x of p.
+template<int D>
+class EdgeDistanceFactor : public gtsam::NoiseModelFactor1<Eigen::Matrix<double, D, 1> >
+{
+	typedef Eigen::Matrix<double, D, 1> X;
+public:
+	EdgeDistanceFactor(gtsam::Key key, const gtsam::SharedNoiseModel & model,
+			const CalibrationProblem & problem, const Frame & frame, size_t point,
+			const std::vector<int> & solved, const double p[6], double cap) :
+		gtsam::NoiseModelFactor1<X>(model, key),
+		problem_(problem), frame_(frame), point_(point), solved_(solved), cap_(cap)
+	{
+		std::copy(p, p + 6, p_);
+	}
+
+	gtsam::Vector evaluateError(const X & x,
+#if GTSAM_VERSION_NUMERIC >= 40300
+			gtsam::OptionalMatrixType H = OptionalNone) const override
+#else
+			boost::optional<gtsam::Matrix &> H = boost::none) const override
+#endif
+	{
+		if(H)
+		{
+			// Central differences, with steps of the precision the projection needs.
+			gtsam::Matrix J(1, D);
+			for(int k = 0; k < D; ++k)
+			{
+				const double h = solved_[k] < 3 ? 1e-4 : 1e-3;  // m, deg
+				X plus = x, minus = x;
+				plus[k] += h;
+				minus[k] -= h;
+				J(0, k) = (residual(plus) - residual(minus)) / (2.0 * h);
+			}
+			*H = J;
+		}
+		return gtsam::Vector1(residual(x));
+	}
+
+private:
+	double residual(const X & x) const
+	{
+		double q[6];
+		std::copy(p_, p_ + 6, q);
+		for(int k = 0; k < D; ++k)
+		{
+			q[solved_[k]] = x[k];
+		}
+		return problem_.edgeDistance(frame_, point_, q, cap_);
+	}
+
+	const CalibrationProblem & problem_;
+	const Frame & frame_;
+	size_t point_;
+	std::vector<int> solved_;
+	double p_[6];
+	double cap_;
+};
+
+}  // namespace
+
+void GtsamSolver::solve(const CalibrationProblem & problem, bool estimateTranslation, double p[6]) const
+{
+	coarse_.solve(problem, estimateTranslation, p);
+	if(estimateTranslation)
+	{
+		solveWith<6>(problem, {0, 1, 2, 3, 4, 5}, p);
+	}
+	else
+	{
+		solveWith<3>(problem, {3, 4, 5}, p);
+	}
+}
+
+template<int D>
+void GtsamSolver::solveWith(const CalibrationProblem & problem, const std::vector<int> & solved, double p[6]) const
+{
+	typedef Eigen::Matrix<double, D, 1> X;
+	const gtsam::Key key = 0;
+
+	X x;
+	for(int k = 0; k < D; ++k)
+	{
+		x[k] = p[solved[k]];
+	}
+	for(double scale : {9.0, 3.0, 1.0})
+	{
+		const double delta = scale * sigma_;
+		const double cap = 10.0 * delta;  // beyond a few times the scale, a point does not count anyway
+		const gtsam::noiseModel::mEstimator::Welsch::shared_ptr welsch =
+				gtsam::noiseModel::mEstimator::Welsch::Create(delta);
+		gtsam::NonlinearFactorGraph graph;
+		for(int k : problem.nodes())
+		{
+			const Frame & f = problem.frames()[k];
+			for(size_t i = 0; i < f.edgePoints.size(); ++i)
+			{
+				// Information = the point's weight, as with g2o.
+				const gtsam::SharedNoiseModel model = gtsam::noiseModel::Robust::Create(
+						welsch, gtsam::noiseModel::Isotropic::Sigma(1, 1.0 / std::sqrt(f.edgeWeights[i])));
+				graph.emplace_shared<EdgeDistanceFactor<D> >(key, model, problem, f, i, solved, p, cap);
+			}
+		}
+		gtsam::Values initial;
+		initial.insert(key, x);
+		gtsam::LevenbergMarquardtParams params;
+		params.setMaxIterations(10);  // close already, after the coarse search
+		x = gtsam::LevenbergMarquardtOptimizer(graph, initial, params).optimize().template at<X>(key);
+	}
+	for(int k = 0; k < D; ++k)
+	{
+		p[solved[k]] = x[k];
+	}
+}
+
+#endif

tools/LidarCameraCalibration/GtsamSolver.h

@@ -0,0 +1,58 @@
+/*
+Copyright (c) 2010-2026, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+    * Redistributions of source code must retain the above copyright
+      notice, this list of conditions and the following disclaimer.
+    * Redistributions in binary form must reproduce the above copyright
+      notice, this list of conditions and the following disclaimer in the
+      documentation and/or other materials provided with the distribution.
+    * Neither the name of the Universite de Sherbrooke nor the
+      names of its contributors may be used to endorse or promote products
+      derived from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
+DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+#ifndef LIDARCAMERACALIBRATION_GTSAMSOLVER_H_
+#define LIDARCAMERACALIBRATION_GTSAMSOLVER_H_
+
+#include "CorrectionSolver.h"
+#include "PatternSearchSolver.h"
+
+#include <rtabmap/core/Version.h>
+
+#ifdef RTABMAP_GTSAM
+
+// The same least squares as G2oSolver (see there), with GTSAM: one factor per lidar edge
+// point on the correction's parameters, a Welsch robust noise model, Levenberg-Marquardt
+// after a coarse pattern search, with the robust scale going from wide to narrow.
+class GtsamSolver : public CorrectionSolver
+{
+public:
+	GtsamSolver(double sigma) : sigma_(sigma), coarse_(4) {}
+	const char * name() const override {return "gtsam";}
+	void solve(const CalibrationProblem & problem, bool estimateTranslation, double p[6]) const override;
+
+private:
+	template<int D>
+	void solveWith(const CalibrationProblem & problem, const std::vector<int> & solved, double p[6]) const;
+
+	double sigma_;
+	PatternSearchSolver coarse_;  // steps of 2 down to 0.25 deg
+};
+
+#endif
+
+#endif /* LIDARCAMERACALIBRATION_GTSAMSOLVER_H_ */

tools/LidarCameraCalibration/PatternSearchSolver.cpp

@@ -0,0 +1,73 @@
+/*
+Copyright (c) 2010-2026, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+    * Redistributions of source code must retain the above copyright
+      notice, this list of conditions and the following disclaimer.
+    * Redistributions in binary form must reproduce the above copyright
+      notice, this list of conditions and the following disclaimer in the
+      documentation and/or other materials provided with the distribution.
+    * Neither the name of the Universite de Sherbrooke nor the
+      names of its contributors may be used to endorse or promote products
+      derived from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
+DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+#include "PatternSearchSolver.h"
+
+#include <algorithm>
+
+void PatternSearchSolver::solve(const CalibrationProblem & problem, bool estimateTranslation, double p[6]) const
+{
+	double steps[6];
+	std::copy(kInitialSteps, kInitialSteps + 6, steps);
+	if(!estimateTranslation)
+	{
+		steps[0] = steps[1] = steps[2] = 0.0;
+	}
+	double best = problem.score(correctionFrom(p));
+	for(int level = 0; level < levels_; ++level)
+	{
+		bool improved = true;
+		while(improved)
+		{
+			improved = false;
+			for(int k = 0; k < 6; ++k)
+			{
+				if(steps[k] == 0.0)
+				{
+					continue;
+				}
+				for(int sign = -1; sign <= 1; sign += 2)
+				{
+					double q[6];
+					std::copy(p, p + 6, q);
+					q[k] += sign * steps[k];
+					const double s = problem.score(correctionFrom(q));
+					if(s > best + 1e-7)
+					{
+						best = s;
+						std::copy(q, q + 6, p);
+						improved = true;
+					}
+				}
+			}
+		}
+		for(double & s : steps)
+		{
+			s /= 2.0;
+		}
+	}
+}

tools/LidarCameraCalibration/PatternSearchSolver.h

@@ -0,0 +1,47 @@
+/*
+Copyright (c) 2010-2026, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+    * Redistributions of source code must retain the above copyright
+      notice, this list of conditions and the following disclaimer.
+    * Redistributions in binary form must reproduce the above copyright
+      notice, this list of conditions and the following disclaimer in the
+      documentation and/or other materials provided with the distribution.
+    * Neither the name of the Universite de Sherbrooke nor the
+      names of its contributors may be used to endorse or promote products
+      derived from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
+DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+#ifndef LIDARCAMERACALIBRATION_PATTERNSEARCHSOLVER_H_
+#define LIDARCAMERACALIBRATION_PATTERNSEARCHSOLVER_H_
+
+#include "CorrectionSolver.h"
+
+// One parameter at a time, trying a step each way, until no step improves, then with
+// half the steps. Simple and robust, but can only move along the parameters' axes.
+class PatternSearchSolver : public CorrectionSolver
+{
+public:
+	// levels: how many times the steps are halved, from 2 deg (7: down to about 0.03 deg)
+	PatternSearchSolver(int levels = 7) : levels_(levels) {}
+	const char * name() const override {return "pattern";}
+	void solve(const CalibrationProblem & problem, bool estimateTranslation, double p[6]) const override;
+
+private:
+	int levels_;
+};
+
+#endif /* LIDARCAMERACALIBRATION_PATTERNSEARCHSOLVER_H_ */

tools/LidarCameraCalibration/README.md

@@ -0,0 +1,137 @@
+# rtabmap-lidarCameraCalibration
+
+Refines the rotation between a camera and a lidar from a mapping session, without a calibration target. It aligns the edges the lidar sees (depth discontinuities, creases and reflectance changes) with the edges in the camera images, over all the nodes of an RTAB-Map database at once.
+
+```bash
+rtabmap-lidarCameraCalibration [options] map.db
+```
+
+The database is opened read-only. The tool prints a correction to apply to the camera's transform; it does not modify anything.
+
+## What the database needs
+
+Each node must have an image from one camera and a lidar scan taken at about the same time, with the camera's and the lidar's local transforms (from TF when mapping with ROS).
+
+What helps:
+
+- **Dense scans.** Edges are found where the scan, projected in the image, leaves no holes. An assembled cloud (a few lidar sweeps per node) at full resolution, or with a small voxel (2 cm), works better than a sparse one.
+- **Images and scans taken together.** The tool assumes that the camera's and the lidar's clocks are synchronized: it does not estimate a time offset between them, which would look like a rotation while the robot turns. With `odom_sensor_sync`, the camera's transform stored in each node already compensates the motion between the image's and the scan's stamps. The tool estimates its correction on top of that.
+- **Many nodes from varied viewpoints.** The correction is the one that fits all the nodes; a single view constrains it much less.
+- **Intensity in the scans** (format `XYZI`). It is used when there; see below.
+
+## How it works
+
+For every node:
+
+1. **Image edges.** Canny edges of the image, turned into a score with a distance transform: 1 on an edge, decreasing with the distance to it (`exp(-d / sigma)`, `--sigma`, default 3 pixels). The score is smooth enough for a search to follow it toward the edges.
+2. **Lidar edges.** The scan is projected in the camera at a coarse resolution (`--decimation`, default 1/4 of the image), where the projected points are dense, keeping the nearest point per cell so that surfaces hidden from the camera do not count. A cell's point is a lidar edge point when:
+   - it is in front of a depth discontinuity: a neighboring cell is at least 15% farther (`--jump`) than where this cell's surface would continue, as at the border of an object in front of a farther background. The continuation is predicted from the opposite neighbor (on a plane, inverse depth is linear in the image), so that a surface seen at a grazing angle, such as the floor ahead of a low camera, is not taken for a discontinuity: its depth changes fast from one cell to the next, but as its continuation predicts; or
+   - it is on an intensity edge: the lidar's intensity changes by more than 40% to a neighboring cell (`--intensity_jump`), as at a change of paint or material. A cell's intensity is the mean (of the logarithm) over all the points of the surface it sees, not its nearest point's: a lidar's beams do not return the same intensity from the same surface, and a node's scan assembles several sweeps, so cells seen by different beams would otherwise differ and make false edges along the beams' traces, even on a flat uniform wall. Then it is median filtered against what speckle remains. As for creases (below), the cells only say where there is an intensity edge: where exactly is found at the image's full resolution. This is what finds edges on flat surfaces, where the depth does not jump: holds on a climbing wall, window frames, panels. `--no_intensity` turns it off; or
+   - it is on a crease: the surface normals of neighboring cells differ by more than 45 degrees (`--crease_angle`, 0 disables), as between a wall and the floor, where the depth does not jump. Normals are computed on the scan voxelized (`--crease_voxel`, 10 cm), smoother than at full resolution; each voxel's normal is spread over the cells it covers, and averaged per cell like intensity. As the normals then change over a band of a few cells across a crease (and intensity across an intensity edge), where exactly it is is found at the image's full resolution, as Canny finds edges: the normals (or intensity) are interpolated and smoothed, and the edge is where their change is the largest across it, to a fraction of a pixel. Its depth is interpolated there (these edges are on continuous surfaces), and the point is put back in 3D, one per cell. Taking the nearest lidar point of each cell instead would put them anywhere in a band a few cells wide on both sides of the edge. They help most where surfaces all reflect alike, with few intensity edges.
+
+   Each point is weighted by the size of its jump. The edge points are kept in 3D, in the scan's frame, so that they can then be projected at full resolution.
+
+Then a solver finds the correction `C` of the camera's transform that best lines the lidar edge points up with the images' edges, over all the nodes, starting from no correction (`--solver`). The direct solvers maximize a score: the weighted average, over all the lidar edge points, of the image edge score where the point projects at full resolution (interpolated).
+
+- `simplex` (default): OpenCV's Nelder-Mead simplex (`cv::DownhillSolver`), started again once from its result. It moves all the parameters together, so it can follow directions in which they are coupled (which is likely between translation and rotation).
+- `pattern`: one parameter at a time, a step each way, until no step improves, then with half the steps (2 degrees down to about 0.03 degree). The fastest, but it moves only along the parameters' axes.
+
+The least-squares solvers (if RTAB-Map is built with g2o or GTSAM) minimize instead, for each lidar edge point, its distance in pixels to the nearest image edge where it projects (chamfer matching), weighted by the point's weight:
+
+- `g2o`, `gtsam`: Levenberg-Marquardt, with a Welsch robust kernel, which makes a point far from any image edge count for nothing, as the score does: many lidar edges have no counterpart in the image. Levenberg-Marquardt only follows the local slope, and each point is pulled toward its nearest image edge, often not its own when far from the solution, so on its own it stops in a local minimum a few degrees away. A coarse `pattern` search (2 down to 0.25 degree) gets close first, then the kernel's scale goes from wide to narrow (9, 3, then 1 x `--sigma`). Several times longer than `simplex`, and about 2 GB of memory for 500k lidar edge points.
+
+The lidar edge points are then selected again with the result, and the solver runs a second time from there.
+
+### Code
+
+- `main.cpp`: loading the database, the image edges, the lidar edge points (`selectEdgePoints()`), the passes, the checks and the report.
+- `CalibrationProblem.h/.cpp`: what a solver sees: the nodes' lidar edge points, where a correction projects them (`project()`), the score (`score()`), and the distance to the image edges for least squares (`edgeDistance()`).
+- `CorrectionSolver.h/.cpp`: the solvers' interface and `createSolver()`.
+- `PatternSearchSolver`, `DownhillSimplexSolver`, `G2oSolver`, `GtsamSolver` (`.h/.cpp`): the solvers. The last two compile to nothing when RTAB-Map is built without g2o or GTSAM.
+
+A new solver implements `CorrectionSolver` and is added to `createSolver()` and `availableSolvers()`.
+
+This is the approach of Levinson and Thrun (see [Reference](#reference)), with intensity edges added.
+
+## The result
+
+The tool prints a correction `X` of the camera's mount, in the camera's body frame (x forward, y left, z up): the robot base to camera transform `B` (e.g., `base_link -> camera_link`) becomes `B * X`. It is a multiplication of transforms, not a sum of angles. Its roll is about the camera's viewing axis, its pitch is the camera's tilt and its yaw its pan. It prints it in radians and degrees, and as a quaternion. In TF, `X` can also be inserted after `B`, leaving the other transforms as they are: `base_link -[B]-> camera_link_measured -[X]-> camera_link`.
+
+Internally, the lidar is projected in the camera's optical frame (x right, y down, z forward), under the body frame through the optical rotation `R` (roll -pi/2, pitch 0, yaw -pi/2): the solvers search for the same correction there, `C = R^-1 * X * R`, each node's camera local transform `T` becoming `T * C`.
+
+### Rotation only, by default
+
+Only the rotation is estimated unless `--translation` is given. The translation between a camera and a lidar is usually a few centimeters, which moves the edges in the images very little when the scene is several meters away: it is not observable, and estimating it anyway gives values that change from one subset of the nodes to another. Measure it instead, or estimate it with `--translation` only on data with close surfaces, and check it as below.
+
+### Checking it
+
+The tool prints, for each kind of lidar edge, the share of its points within 2 pixels of an image edge once corrected: how much each brings, and how much of it is noise. Then:
+
+- **Each half of the nodes on its own.** The even and the odd nodes are calibrated separately. If they agree, the result is supported by the data; if they differ, the difference is about how much the result can be trusted.
+- **The sensitivity**, with `--verbose`: how much the score drops, in percent, with the result off by 1 degree (roll, pitch, yaw) or 2 cm (x, y, z) along or about each axis of the camera, the mean of both directions. Think of the score as a valley with the result at its bottom: the sensitivity is how steep its sides are along each axis.
+  - A large drop (several percent or more for 1 degree) means the data determines that axis well: a small error on it would misalign many edges, so the solver cannot be far off, and a correction on that axis can be trusted, however large.
+  - Almost none (around 1% or less) means the axis is not observable from this data: the edges hardly move with it, so any value the solver finds on it, large or small, is not reliable.
+  - It says how sure the result is, not how far off the camera was: that is the correction itself. It depends on the scene and the sensors, not on the error: e.g., many vertical and horizontal edges make yaw and pitch steep, roll (about the viewing axis) moves edges little near the image center so it is usually less steep, and translation is flat unless surfaces are close (a 2 cm shift moves edges a fraction of a pixel at several meters).
+
+With `--images dir`, the image of every node is saved darkened, with the image edges the alignment uses (Canny) in green and the lidar edge points projected over them, depth discontinuities in red, creases in blue and intensity edges in yellow, before (`<id>_edges_1_before.png`, from where the search started: the database's camera transform, with `--initial_rotation` if given) and after (`<id>_edges_2_after.png`) the correction, with the node's id and speeds in the top left corner: the mean since the previous node (over which an assembled scan is taken) and the instantaneous one when the node was added. After, the lidar points should lie on green wherever both sensors see an edge. Points away from any green, and green edges without points, are edges only one of the sensors sees (e.g., lidar intensity through glass, or shadows in the image).
+
+For example, a node of an indoor climbing gym, before (left) and after (right) the correction: before, the creases along the floor and the climbing holds' outlines are off their green edges; after, they lie on them.
+
+| Before (`<id>_edges_1_before.png`) | After (`<id>_edges_2_after.png`) |
+|---|---|
+| ![before](images/1621_edges_1_before.jpg) | ![after](images/1621_edges_2_after.jpg) |
+
+With each of them, two images show the maps the lidar edges are found from, at the decimated resolution, half transparent over the image, with the image's edges in green and the map's own edges (Canny, as for the image) in blue:
+
+- `<id>_intensity_1_before.png`, `<id>_intensity_2_after.png`: the lidar's intensity per cell, as used (mean log-intensity, median filtered), from red (dark) to yellow (bright), with the contrast stretched for each image. The intensity edges are where it changes; they should line up with green where the image shows the same change of material.
+- `<id>_normals_1_before.png`, `<id>_normals_2_after.png`: how each cell's surface faces the camera, from yellow (facing it) to red (seen edge on, at a grazing angle). Surfaces at a grazing angle are where depth changes fast without a discontinuity, and where the intensity drops. The normals are computed on the scans voxelized at `--crease_voxel`, so this image is made whether or not creases are used.
+
+Before (left) and after (right) the correction: the intensity of the same node, where the holds stand out in yellow, and the normals of another, where the climbing wall seen at a grazing angle is red against the walls facing the camera in yellow. Once corrected, the map's edges (blue) lie on the image's (green).
+
+| Before | After |
+|---|---|
+| ![intensity before](images/1621_intensity_1_before.jpg) | ![intensity after](images/1621_intensity_2_after.jpg) |
+| ![normals before](images/2093_normals_1_before.jpg) | ![normals after](images/2093_normals_2_after.jpg) |
+
+It is also worth running it again with other values of `--decimation`, `--intensity_jump` or `--voxel` (which voxel filters the scans first, to compare densities): a result that does not move with them is more trustworthy than one that does.
+
+## Options
+
+| Option | Default | Description |
+|---|---|---|
+| `--solver "name"` | `simplex` | `simplex`, `pattern`, `g2o` or `gtsam`, see above. `--help` lists those in this build. |
+| `--verbose` | off | Also print the sensitivity (see above). |
+| `--translation` | off | Also estimate the translation (see above). |
+| `--images "dir"` | | Save the image of every node with its edges (green) and the lidar edge points (depth: red, crease: blue, intensity: yellow), before and after the correction, and the lidar's intensity and surface orientation (see above). |
+| `--decimation #` | `4` | Image decimation at which the lidar edges are found. Lower is finer, but needs denser scans. |
+| `--jump #.#` | `0.15` | Relative depth jump for a depth discontinuity, as a fraction of the point's depth: 0.15 means a neighbor at least 15% of its depth farther than where its surface would continue (e.g., 0.6 m behind a point at 4 m). |
+| `--intensity_jump #.#` | `0.4` | Relative intensity change for an intensity edge, as a fraction: 0.4 means a neighbor at least 1.4 times brighter or darker (compared on log-intensity). Lower finds more edges, and more speckle. |
+| `--intensity_weight #.#` | `1` | Weight of the intensity edges relative to the depth discontinuities. Lower it where intensity is less reliable than geometry (e.g., much glass). |
+| `--crease_angle #.#` | `45` | Creases: where the surface normals differ by this angle (deg). 0: off. |
+| `--crease_voxel #.#` | `0.1` | Voxel size (m) of the scans on which the normals are computed. |
+| `--no_intensity` | | Use only depth discontinuities. |
+| `--sigma #.#` | `3` | Fall off, in pixels, of the image edge score. |
+| `--min_depth #.#` | `0.5` | Ignore lidar points closer than this to the camera (m). |
+| `--initial_rotation #.# #.# #.#` | `0 0 0` | Start from this correction (roll, pitch, yaw in deg, in the camera's body frame) instead of none. The correction printed stays relative to the database's camera transform (the same as without this option if the result is found again); the one relative to the starting point is printed too: to see from how far off the result is found again, or to start closer when the stored transform is known to be far off. The two halves of the nodes start from it too. |
+| `--max_angular_speed #.#` | `0` | Skip the nodes rotating faster than this (deg/s): the mean since the previous node, from their odometry poses, over which an assembled scan is taken (else the instantaneous odometry velocity stored in the node). 0: keep all. |
+| `--max_linear_speed #.#` | `0` | Skip the nodes moving faster than this (m/s). 0: keep all. |
+| `--voxel #.#` | `0` | Voxel filter the scans first (m), to compare the result at several lidar densities. |
+
+## Limits
+
+- **Time offset.** A delay between the camera's and the lidar's clocks looks like a rotation while the robot turns. It is not estimated: use well synchronized sensors, or data where the robot turns slowly (`--max_angular_speed` skips the nodes where it turns fast).
+- **Motion within a node.** An assembled cloud spans a few sweeps; the points from the earlier ones are moved by odometry, whose error adds noise.
+- **Rolling shutter.** It is not modeled; fast rotations distort the images.
+- **Intrinsics.** The camera's calibration (focal lengths, center, distortion) is assumed right; an error there biases the rotation.
+- **One camera per node.** Nodes with several cameras are skipped.
+
+## Reference
+
+J. Levinson and S. Thrun, "Automatic Online Calibration of Cameras and Lasers", *Robotics: Science and Systems IX* (RSS), 2013. [PDF](http://www.roboticsproceedings.org/rss09/p29.pdf)
+
+What this tool takes from it: lidar points at depth discontinuities as the lidar's edges, the image's edges spread out by a distance transform so that the alignment score is smooth, and that score maximized over many frames at once. What it does differently:
+
+- The depth discontinuities are found in the scan projected in the camera, at a coarse resolution, rather than between consecutive points of a scan line: the scans of a node can be an assembly of several sweeps, voxel filtered, without scan lines. They are measured against the continuation of the surface, so that surfaces seen at a grazing angle do not make false ones.
+- Intensity edges and creases are added to the depth discontinuities, for surfaces without depth jumps.
+- The search is local, from the current transform, with a choice of solvers; the rotation only by default.
+- The result is checked on two halves of the nodes estimated separately.