examples/slam/pose_graph_3d/pose_graph_3d_error_term.h

   1 // Ceres Solver - A fast non-linear least squares minimizer
   2 // Copyright 2016 Google Inc. All rights reserved.
   3 // http://ceres-solver.org/
   4 //
   5 // Redistribution and use in source and binary forms, with or without
   6 // modification, are permitted provided that the following conditions are met:
   7 //
   8 // * Redistributions of source code must retain the above copyright notice,
   9 //   this list of conditions and the following disclaimer.
  10 // * Redistributions in binary form must reproduce the above copyright notice,
  11 //   this list of conditions and the following disclaimer in the documentation
  12 //   and/or other materials provided with the distribution.
  13 // * Neither the name of Google Inc. nor the names of its contributors may be
  14 //   used to endorse or promote products derived from this software without
  15 //   specific prior written permission.
  16 //
  17 // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
  18 // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
  19 // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
  20 // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
  21 // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
  22 // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
  23 // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
  24 // INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
  25 // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
  26 // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
  27 // POSSIBILITY OF SUCH DAMAGE.
  28 //
  29 // Author: vitus@google.com (Michael Vitus)
  30
  31 #ifndef EXAMPLES_CERES_POSE_GRAPH_3D_ERROR_TERM_H_
  32 #define EXAMPLES_CERES_POSE_GRAPH_3D_ERROR_TERM_H_
  33
  34 #include "Eigen/Core"
  35 #include "ceres/autodiff_cost_function.h"
  36
  37 #include "types.h"
  38
  39 namespace ceres {
  40 namespace examples {
  41
  42 // Computes the error term for two poses that have a relative pose measurement
  43 // between them. Let the hat variables be the measurement. We have two poses x_a
  44 // and x_b. Through sensor measurements we can measure the transformation of
  45 // frame B w.r.t frame A denoted as t_ab_hat. We can compute an error metric
  46 // between the current estimate of the poses and the measurement.
  47 //
  48 // In this formulation, we have chosen to represent the rigid transformation as
  49 // a Hamiltonian quaternion, q, and position, p. The quaternion ordering is
  50 // [x, y, z, w].
  51
  52 // The estimated measurement is:
  53 //      t_ab = [ p_ab ]  = [ R(q_a)^T * (p_b - p_a) ]
  54 //             [ q_ab ]    [ q_a^{-1] * q_b         ]
  55 //
  56 // where ^{-1} denotes the inverse and R(q) is the rotation matrix for the
  57 // quaternion. Now we can compute an error metric between the estimated and
  58 // measurement transformation. For the orientation error, we will use the
  59 // standard multiplicative error resulting in:
  60 //
  61 //   error = [ p_ab - \hat{p}_ab                 ]
  62 //           [ 2.0 * Vec(q_ab * \hat{q}_ab^{-1}) ]
  63 //
  64 // where Vec(*) returns the vector (imaginary) part of the quaternion. Since
  65 // the measurement has an uncertainty associated with how accurate it is, we
  66 // will weight the errors by the square root of the measurement information
  67 // matrix:
  68 //
  69 //   residuals = I^{1/2) * error
  70 // where I is the information matrix which is the inverse of the covariance.
  71 class PoseGraph3dErrorTerm {
  72  public:
  73   PoseGraph3dErrorTerm(const Pose3d& t_ab_measured,
  74                        const Eigen::Matrix<double, 6, 6>& sqrt_information)
  75       : t_ab_measured_(t_ab_measured), sqrt_information_(sqrt_information) {}
  76
  77   template <typename T>
  78   bool operator()(const T* const p_a_ptr, const T* const q_a_ptr,
  79                   const T* const p_b_ptr, const T* const q_b_ptr,
  80                   T* residuals_ptr) const {
  81     Eigen::Map<const Eigen::Matrix<T, 3, 1> > p_a(p_a_ptr);
  82     Eigen::Map<const Eigen::Quaternion<T> > q_a(q_a_ptr);
  83
  84     Eigen::Map<const Eigen::Matrix<T, 3, 1> > p_b(p_b_ptr);
  85     Eigen::Map<const Eigen::Quaternion<T> > q_b(q_b_ptr);
  86
  87     // Compute the relative transformation between the two frames.
  88     Eigen::Quaternion<T> q_a_inverse = q_a.conjugate();
  89     Eigen::Quaternion<T> q_ab_estimated = q_a_inverse * q_b;
  90
  91     // Represent the displacement between the two frames in the A frame.
  92     Eigen::Matrix<T, 3, 1> p_ab_estimated = q_a_inverse * (p_b - p_a);
  93
  94     // Compute the error between the two orientation estimates.
  95     Eigen::Quaternion<T> delta_q =
  96         t_ab_measured_.q.template cast<T>() * q_ab_estimated.conjugate();
  97
  98     // Compute the residuals.
  99     // [ position         ]   [ delta_p          ]
 100     // [ orientation (3x1)] = [ 2 * delta_q(0:2) ]
 101     Eigen::Map<Eigen::Matrix<T, 6, 1> > residuals(residuals_ptr);
 102     residuals.template block<3, 1>(0, 0) =
 103         p_ab_estimated - t_ab_measured_.p.template cast<T>();
 104     residuals.template block<3, 1>(3, 0) = T(2.0) * delta_q.vec();
 105
 106     // Scale the residuals by the measurement uncertainty.
 107     residuals.applyOnTheLeft(sqrt_information_.template cast<T>());
 108
 109     return true;
 110   }
 111
 112   static ceres::CostFunction* Create(
 113       const Pose3d& t_ab_measured,
 114       const Eigen::Matrix<double, 6, 6>& sqrt_information) {
 115     return new ceres::AutoDiffCostFunction<PoseGraph3dErrorTerm, 6, 3, 4, 3, 4>(
 116         new PoseGraph3dErrorTerm(t_ab_measured, sqrt_information));
 117   }
 118
 119   EIGEN_MAKE_ALIGNED_OPERATOR_NEW
 120
 121  private:
 122   // The measurement for the position of B relative to A in the A frame.
 123   const Pose3d t_ab_measured_;
 124   // The square root of the measurement information matrix.
 125   const Eigen::Matrix<double, 6, 6> sqrt_information_;
 126 };
 127
 128 }  // namespace examples
 129 }  // namespace ceres
 130
 131 #endif  // EXAMPLES_CERES_POSE_GRAPH_3D_ERROR_TERM_H_