g2o
closed_form_calibration.cpp
Go to the documentation of this file.
1 // g2o - General Graph Optimization
2 // Copyright (C) 2011 R. Kuemmerle, G.Grisetti, W. Burgard
3 // All rights reserved.
4 //
5 // Redistribution and use in source and binary forms, with or without
6 // modification, are permitted provided that the following conditions are
7 // met:
8 //
9 // * Redistributions of source code must retain the above copyright notice,
10 // this list of conditions and the following disclaimer.
11 // * Redistributions in binary form must reproduce the above copyright
12 // notice, this list of conditions and the following disclaimer in the
13 // documentation and/or other materials provided with the distribution.
14 //
15 // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS
16 // IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED
17 // TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A
18 // PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
19 // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
20 // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED
21 // TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
22 // PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
23 // LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
24 // NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
25 // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
26 
28 
30 
31 #include <iostream>
32 #include <limits>
33 
34 #include <Eigen/SVD>
35 
36 #define SQR(X) ( std::pow(X,2) )
37 #define CUBE(X) ( std::pow(X,3) )
38 
39 using namespace Eigen;
40 
41 namespace g2o {
42 
43 bool ClosedFormCalibration::calibrate(const MotionInformationVector& measurements, SE2& laserOffset, Eigen::Vector3d& odomParams)
44 {
45  std::vector<VelocityMeasurement, Eigen::aligned_allocator<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(), odomMotion.translation().y(), odomMotion.rotation().angle(), timeInterval);
50  VelocityMeasurement velMeas = OdomConvert::convertToVelocity(mm);
51  velMeasurements.push_back(velMeas);
52  }
53 
54  double J_21, J_22;
55  {
56  Eigen::MatrixXd A(measurements.size(), 2);
57  Eigen::VectorXd x(measurements.size());
58  for (size_t i = 0; i < measurements.size(); ++i) {
59  const SE2& laserMotion = measurements[i].laserMotion;
60  const double& timeInterval = measurements[i].timeInterval;
61  const VelocityMeasurement& velMeas = velMeasurements[i];
62  A(i, 0) = velMeas.vl() * timeInterval;
63  A(i, 1) = velMeas.vr() * timeInterval;
64  x(i) = laserMotion.rotation().angle();
65  }
66  // (J_21, J_22) = (-r_l / b, r_r / b)
67  Eigen::Vector2d linearSolution = (A.transpose() * A).inverse() * A.transpose() * x;
68  //std::cout << linearSolution.transpose() << std::endl;
69  J_21 = linearSolution(0);
70  J_22 = linearSolution(1);
71  }
72 
73  // construct M
74  Eigen::Matrix<double, 5, 5> M;
75  M.setZero();
76  for (size_t i = 0; i < measurements.size(); ++i) {
77  const SE2& laserMotion = measurements[i].laserMotion;
78  const double& timeInterval = measurements[i].timeInterval;
79  const VelocityMeasurement& velMeas = velMeasurements[i];
80  Eigen::Matrix<double, 2, 5> L;
81  double omega_o_k = J_21 * velMeas.vl() + J_22 * velMeas.vr();
82  double o_theta_k = omega_o_k * timeInterval;
83  double sx = 1.;
84  double sy = 0.;
85  if (fabs(o_theta_k) > std::numeric_limits<double>::epsilon()) {
86  sx = sin(o_theta_k) / o_theta_k;
87  sy = (1 - cos(o_theta_k)) / o_theta_k;
88  }
89  double c_x = 0.5 * timeInterval * (-J_21 * velMeas.vl() + J_22 * velMeas.vr()) * sx;
90  double c_y = 0.5 * timeInterval * (-J_21 * velMeas.vl() + J_22 * velMeas.vr()) * sy;
91  L(0, 0) = -c_x;
92  L(0, 1) = 1 - cos(o_theta_k);
93  L(0, 2) = sin(o_theta_k);
94  L(0, 3) = laserMotion.translation().x();
95  L(0, 4) = -laserMotion.translation().y();
96  L(1, 0) = -c_y;
97  L(1, 1) = - sin(o_theta_k);
98  L(1, 2) = 1 - cos(o_theta_k);
99  L(1, 3) = laserMotion.translation().y();
100  L(1, 4) = laserMotion.translation().x();
101  M.noalias() += L.transpose() * L;
102  }
103  //std::cout << M << std::endl;
104 
105  // compute lagrange multiplier
106  // and solve the constrained least squares problem
107  double m11 = M(0,0);
108  double m13 = M(0,2);
109  double m14 = M(0,3);
110  double m15 = M(0,4);
111  double m22 = M(1,1);
112  double m34 = M(2,3);
113  double m35 = M(2,4);
114  double m44 = M(3,3);
115 
116  double a = m11 * SQR(m22) - m22 * SQR(m13);
117  double b = 2*m11 * SQR(m22) * m44 - SQR(m22) * SQR(m14) - 2*m22 * SQR(m13) * m44 - 2*m11 * m22 * SQR(m34)
118  - 2*m11 * m22 * SQR(m35) - SQR(m22) * SQR(m15) + 2*m13 * m22 * m34 * m14 + SQR(m13) * SQR(m34)
119  + 2*m13 * m22 * m35 * m15 + SQR(m13) * SQR(m35);
120  double c = - 2*m13 * CUBE(m35) * m15 - m22 * SQR(m13) * SQR(m44) + m11 * SQR(m22) * SQR(m44) + SQR(m13) * SQR(m35) * m44
121  + 2*m13 * m22 * m34 * m14 * m44 + SQR(m13) * SQR(m34) * m44 - 2*m11 * m22 * SQR(m34) * m44 - 2 * m13 * CUBE(m34) * m14
122  - 2*m11 * m22 * SQR(m35) * m44 + 2*m11 * SQR(m35) * SQR(m34) + m22 * SQR(m14) * SQR(m35) - 2*m13 * SQR(m35) * m34 * m14
123  - 2*m13 * SQR(m34) * m35 * m15 + m11 * std::pow(m34, 4) + m22 * SQR(m15) * SQR(m34) + m22 * SQR(m35) * SQR(m15)
124  + m11 * std::pow(m35, 4) - SQR(m22) * SQR(m14) * m44 + 2*m13 * m22 * m35 * m15 * m44 + m22 * SQR(m34) * SQR(m14)
125  - SQR(m22) * SQR(m15) * m44;
126 
127  // solve the quadratic equation
128  double lambda1, lambda2;
129  if(a < std::numeric_limits<double>::epsilon()) {
130  if(b <= std::numeric_limits<double>::epsilon())
131  return false;
132  lambda1 = lambda2 = -c/b;
133  } else {
134  double delta = b*b - 4*a*c;
135  if (delta < 0)
136  return false;
137  lambda1 = 0.5 * (-b-sqrt(delta)) / a;
138  lambda2 = 0.5 * (-b+sqrt(delta)) / a;
139  }
140 
141  Eigen::VectorXd x1 = solveLagrange(M, lambda1);
142  Eigen::VectorXd x2 = solveLagrange(M, lambda2);
143  double err1 = x1.dot(M * x1);
144  double err2 = x2.dot(M * x2);
145 
146  const Eigen::VectorXd& calibrationResult = err1 < err2 ? x1 : x2;
147  odomParams(0) = - calibrationResult(0) * J_21;
148  odomParams(1) = calibrationResult(0) * J_22;
149  odomParams(2) = calibrationResult(0);
150 
151  laserOffset = SE2(calibrationResult(1), calibrationResult(2), atan2(calibrationResult(4), calibrationResult(3)));
152 
153  return true;
154 }
155 
156 Eigen::VectorXd ClosedFormCalibration::solveLagrange(const Eigen::Matrix<double,5,5>& M, double lambda)
157 {
158  // A = M * lambda*W (see paper)
159  Eigen::Matrix<double,5,5> A;
160  A.setZero();
161  A(3,3) = A(4,4) = lambda;
162  A.noalias() += M;
163 
164  // compute the kernel of A by SVD
165  Eigen::JacobiSVD< Eigen::Matrix<double,5,5> > svd(A, ComputeFullV);
166  Eigen::VectorXd result = svd.matrixV().col(4);
167  //for (int i = 0; i < 5; ++i)
168  //std::cout << "singular value " << i << " " << svd.singularValues()(i) << std::endl;
169  //std::cout << "kernel base " << result << std::endl;
170 
171  // enforce the conditions
172  // x_1 > 0
173  if (result(0) < 0.)
174  result *= -1;
175  // x_4^2 + x_5^2 = 1
176  double scale = sqrt(pow(result(3), 2) + pow(result(4), 2));
177  result /= scale;
178 
179  return result;
180 }
181 
182 } // end namespace
#define CUBE(X)
represent SE2
Definition: se2.h:40
static Eigen::Vector2d linearSolution
#define SQR(X)
const Eigen::Rotation2Dd & rotation() const
rotational component
Definition: se2.h:58
velocity measurement of a differential robot
A 2D odometry measurement expressed as a transformation.
std::vector< MotionInformation, Eigen::aligned_allocator< MotionInformation > > MotionInformationVector
const Vector2D & translation() const
translational component
Definition: se2.h:54