g2o
Loading...
Searching...
No Matches
Static Public Member Functions | Static Protected Member Functions | List of all members
g2o::ClosedFormCalibration Class Reference

Simultaneous calibration of the laser offset and the parameters of a differential drive. More...

#include <closed_form_calibration.h>

Static Public Member Functions

static bool calibrate (const MotionInformationVector &measurements, SE2 &laserOffset, Eigen::Vector3d &odomParams)
 

Static Protected Member Functions

static Eigen::VectorXd solveLagrange (const Eigen::Matrix< double, 5, 5 > &M, double lambda)
 

Detailed Description

Simultaneous calibration of the laser offset and the parameters of a differential drive.

Approach described by Censi et al. Andrea Censi, Luca Marchionni, and Giuseppe Oriolo. Simultaneous maximum-likelihood calibration of robot and sensor parameters. In Proceedings of the IEEE International Conference on Robotics and Automation (ICRA). Pasadena, CA, May 2008.

Definition at line 47 of file closed_form_calibration.h.

Member Function Documentation

◆ calibrate()

bool g2o::ClosedFormCalibration::calibrate ( const MotionInformationVector &  measurements,
SE2 &  laserOffset,
Eigen::Vector3d &  odomParams 
)
static

Definition at line 42 of file closed_form_calibration.cpp.

44 {
45 std::vector<VelocityMeasurement> velMeasurements;
46 for (size_t i = 0; i < measurements.size(); ++i) {
47 const SE2& odomMotion = measurements[i].odomMotion;
48 const double& timeInterval = measurements[i].timeInterval;
49 MotionMeasurement mm(odomMotion.translation().x(),
50 odomMotion.translation().y(),
51 odomMotion.rotation().angle(), timeInterval);
52 VelocityMeasurement velMeas = OdomConvert::convertToVelocity(mm);
53 velMeasurements.push_back(velMeas);
54 }
55
56 double J_21, J_22;
57 {
58 Eigen::MatrixXd A(measurements.size(), 2);
59 Eigen::VectorXd x(measurements.size());
60 for (size_t i = 0; i < measurements.size(); ++i) {
61 const SE2& laserMotion = measurements[i].laserMotion;
62 const double& timeInterval = measurements[i].timeInterval;
63 const VelocityMeasurement& velMeas = velMeasurements[i];
64 A(i, 0) = velMeas.vl() * timeInterval;
65 A(i, 1) = velMeas.vr() * timeInterval;
66 x(i) = laserMotion.rotation().angle();
67 }
68 // (J_21, J_22) = (-r_l / b, r_r / b)
69 Eigen::Vector2d linearSolution =
70 (A.transpose() * A).inverse() * A.transpose() * x;
71 // std::cout << linearSolution.transpose() << std::endl;
72 J_21 = linearSolution(0);
73 J_22 = linearSolution(1);
74 }
75
76 // construct M
77 Eigen::Matrix<double, 5, 5> M;
78 M.setZero();
79 for (size_t i = 0; i < measurements.size(); ++i) {
80 const SE2& laserMotion = measurements[i].laserMotion;
81 const double& timeInterval = measurements[i].timeInterval;
82 const VelocityMeasurement& velMeas = velMeasurements[i];
83 Eigen::Matrix<double, 2, 5> L;
84 double omega_o_k = J_21 * velMeas.vl() + J_22 * velMeas.vr();
85 double o_theta_k = omega_o_k * timeInterval;
86 double sx = 1.;
87 double sy = 0.;
88 if (fabs(o_theta_k) > std::numeric_limits<double>::epsilon()) {
89 sx = sin(o_theta_k) / o_theta_k;
90 sy = (1 - cos(o_theta_k)) / o_theta_k;
91 }
92 double c_x =
93 0.5 * timeInterval * (-J_21 * velMeas.vl() + J_22 * velMeas.vr()) * sx;
94 double c_y =
95 0.5 * timeInterval * (-J_21 * velMeas.vl() + J_22 * velMeas.vr()) * sy;
96 L(0, 0) = -c_x;
97 L(0, 1) = 1 - cos(o_theta_k);
98 L(0, 2) = sin(o_theta_k);
99 L(0, 3) = laserMotion.translation().x();
100 L(0, 4) = -laserMotion.translation().y();
101 L(1, 0) = -c_y;
102 L(1, 1) = -sin(o_theta_k);
103 L(1, 2) = 1 - cos(o_theta_k);
104 L(1, 3) = laserMotion.translation().y();
105 L(1, 4) = laserMotion.translation().x();
106 M.noalias() += L.transpose() * L;
107 }
108 // std::cout << M << std::endl;
109
110 // compute lagrange multiplier
111 // and solve the constrained least squares problem
112 double m11 = M(0, 0);
113 double m13 = M(0, 2);
114 double m14 = M(0, 3);
115 double m15 = M(0, 4);
116 double m22 = M(1, 1);
117 double m34 = M(2, 3);
118 double m35 = M(2, 4);
119 double m44 = M(3, 3);
120
121 double a = m11 * SQR(m22) - m22 * SQR(m13);
122 double b = 2 * m11 * SQR(m22) * m44 - SQR(m22) * SQR(m14) -
123 2 * m22 * SQR(m13) * m44 - 2 * m11 * m22 * SQR(m34) -
124 2 * m11 * m22 * SQR(m35) - SQR(m22) * SQR(m15) +
125 2 * m13 * m22 * m34 * m14 + SQR(m13) * SQR(m34) +
126 2 * m13 * m22 * m35 * m15 + SQR(m13) * SQR(m35);
127 double c = -2 * m13 * CUBE(m35) * m15 - m22 * SQR(m13) * SQR(m44) +
128 m11 * SQR(m22) * SQR(m44) + SQR(m13) * SQR(m35) * m44 +
129 2 * m13 * m22 * m34 * m14 * m44 + SQR(m13) * SQR(m34) * m44 -
130 2 * m11 * m22 * SQR(m34) * m44 - 2 * m13 * CUBE(m34) * m14 -
131 2 * m11 * m22 * SQR(m35) * m44 + 2 * m11 * SQR(m35) * SQR(m34) +
132 m22 * SQR(m14) * SQR(m35) - 2 * m13 * SQR(m35) * m34 * m14 -
133 2 * m13 * SQR(m34) * m35 * m15 + m11 * std::pow(m34, 4) +
134 m22 * SQR(m15) * SQR(m34) + m22 * SQR(m35) * SQR(m15) +
135 m11 * std::pow(m35, 4) - SQR(m22) * SQR(m14) * m44 +
136 2 * m13 * m22 * m35 * m15 * m44 + m22 * SQR(m34) * SQR(m14) -
137 SQR(m22) * SQR(m15) * m44;
138
139 // solve the quadratic equation
140 double lambda1, lambda2;
141 if (a < std::numeric_limits<double>::epsilon()) {
142 if (b <= std::numeric_limits<double>::epsilon()) return false;
143 lambda1 = lambda2 = -c / b;
144 } else {
145 double delta = b * b - 4 * a * c;
146 if (delta < 0) return false;
147 lambda1 = 0.5 * (-b - sqrt(delta)) / a;
148 lambda2 = 0.5 * (-b + sqrt(delta)) / a;
149 }
150
151 Eigen::VectorXd x1 = solveLagrange(M, lambda1);
152 Eigen::VectorXd x2 = solveLagrange(M, lambda2);
153 double err1 = x1.dot(M * x1);
154 double err2 = x2.dot(M * x2);
155
156 const Eigen::VectorXd& calibrationResult = err1 < err2 ? x1 : x2;
157 odomParams(0) = -calibrationResult(0) * J_21;
158 odomParams(1) = calibrationResult(0) * J_22;
159 odomParams(2) = calibrationResult(0);
160
161 laserOffset = SE2(calibrationResult(1), calibrationResult(2),
162 atan2(calibrationResult(4), calibrationResult(3)));
163
164 return true;
165}
g2o::ClosedFormCalibration::solveLagrange
static Eigen::VectorXd solveLagrange(const Eigen::Matrix< double, 5, 5 > &M, double lambda)
Definition closed_form_calibration.cpp:167
g2o::OdomConvert::convertToVelocity
static VelocityMeasurement convertToVelocity(const MotionMeasurement &m)
Definition odometry_measurement.cpp:48
SQR
#define SQR(X)
Definition closed_form_calibration.cpp:35
CUBE
#define CUBE(X)
Definition closed_form_calibration.cpp:36
g2o::ceres::cos
Jet< T, N > cos(const Jet< T, N > &f)
Definition jet.h:452
g2o::ceres::sin
Jet< T, N > sin(const Jet< T, N > &f)
Definition jet.h:465
g2o::ceres::sqrt
Jet< T, N > sqrt(const Jet< T, N > &f)
Definition jet.h:444
g2o::ceres::atan2
Jet< T, N > atan2(const Jet< T, N > &g, const Jet< T, N > &f)
Definition jet.h:730
linearSolution
static Eigen::Vector2d linearSolution
Definition sclam_pure_calibration.cpp:55

References g2o::OdomConvert::convertToVelocity(), CUBE, linearSolution, g2o::SE2::rotation(), solveLagrange(), SQR, g2o::SE2::translation(), g2o::VelocityMeasurement::vl(), and g2o::VelocityMeasurement::vr().

Referenced by main().

◆ solveLagrange()

Eigen::VectorXd g2o::ClosedFormCalibration::solveLagrange ( const Eigen::Matrix< double, 5, 5 > &  M,
double  lambda 
)
staticprotected

Definition at line 167 of file closed_form_calibration.cpp.

168 {
169 // A = M * lambda*W (see paper)
170 Eigen::Matrix<double, 5, 5> A;
171 A.setZero();
172 A(3, 3) = A(4, 4) = lambda;
173 A.noalias() += M;
174
175 // compute the kernel of A by SVD
176 Eigen::JacobiSVD<Eigen::Matrix<double, 5, 5> > svd(A, ComputeFullV);
177 Eigen::VectorXd result = svd.matrixV().col(4);
178 // for (int i = 0; i < 5; ++i)
179 // std::cout << "singular value " << i << " " << svd.singularValues()(i) <<
180 // std::endl; std::cout << "kernel base " << result << std::endl;
181
182 // enforce the conditions
183 // x_1 > 0
184 if (result(0) < 0.) result *= -1;
185 // x_4^2 + x_5^2 = 1
186 double scale = sqrt(pow(result(3), 2) + pow(result(4), 2));
187 result /= scale;
188
189 return result;
190}
g2o::ceres::pow
Jet< T, N > pow(const Jet< T, N > &f, double g)
Definition jet.h:743

Referenced by calibrate().

The documentation for this class was generated from the following files:
Generated on Tue Nov 11 2025 05:18:45 for g2o by doxygen 1.9.8