examples/slam/pose_graph_3d/pose_graph_3d.cc

   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 #include <iostream>
  32 #include <fstream>
  33 #include <string>
  34
  35 #include "ceres/ceres.h"
  36 #include "common/read_g2o.h"
  37 #include "gflags/gflags.h"
  38 #include "glog/logging.h"
  39 #include "pose_graph_3d_error_term.h"
  40 #include "types.h"
  41
  42 DEFINE_string(input, "", "The pose graph definition filename in g2o format.");
  43
  44 namespace ceres {
  45 namespace examples {
  46
  47 // Constructs the nonlinear least squares optimization problem from the pose
  48 // graph constraints.
  49 void BuildOptimizationProblem(const VectorOfConstraints& constraints,
  50                               MapOfPoses* poses, ceres::Problem* problem) {
  51   CHECK(poses != NULL);
  52   CHECK(problem != NULL);
  53   if (constraints.empty()) {
  54     LOG(INFO) << "No constraints, no problem to optimize.";
  55     return;
  56   }
  57
  58   ceres::LossFunction* loss_function = NULL;
  59   ceres::LocalParameterization* quaternion_local_parameterization =
  60       new EigenQuaternionParameterization;
  61
  62   for (VectorOfConstraints::const_iterator constraints_iter =
  63            constraints.begin();
  64        constraints_iter != constraints.end(); ++constraints_iter) {
  65     const Constraint3d& constraint = *constraints_iter;
  66
  67     MapOfPoses::iterator pose_begin_iter = poses->find(constraint.id_begin);
  68     CHECK(pose_begin_iter != poses->end())
  69         << "Pose with ID: " << constraint.id_begin << " not found.";
  70     MapOfPoses::iterator pose_end_iter = poses->find(constraint.id_end);
  71     CHECK(pose_end_iter != poses->end())
  72         << "Pose with ID: " << constraint.id_end << " not found.";
  73
  74     const Eigen::Matrix<double, 6, 6> sqrt_information =
  75         constraint.information.llt().matrixL();
  76     // Ceres will take ownership of the pointer.
  77     ceres::CostFunction* cost_function =
  78         PoseGraph3dErrorTerm::Create(constraint.t_be, sqrt_information);
  79
  80     problem->AddResidualBlock(cost_function, loss_function,
  81                               pose_begin_iter->second.p.data(),
  82                               pose_begin_iter->second.q.coeffs().data(),
  83                               pose_end_iter->second.p.data(),
  84                               pose_end_iter->second.q.coeffs().data());
  85
  86     problem->SetParameterization(pose_begin_iter->second.q.coeffs().data(),
  87                                  quaternion_local_parameterization);
  88     problem->SetParameterization(pose_end_iter->second.q.coeffs().data(),
  89                                  quaternion_local_parameterization);
  90   }
  91
  92   // The pose graph optimization problem has six DOFs that are not fully
  93   // constrained. This is typically referred to as gauge freedom. You can apply
  94   // a rigid body transformation to all the nodes and the optimization problem
  95   // will still have the exact same cost. The Levenberg-Marquardt algorithm has
  96   // internal damping which mitigates this issue, but it is better to properly
  97   // constrain the gauge freedom. This can be done by setting one of the poses
  98   // as constant so the optimizer cannot change it.
  99   MapOfPoses::iterator pose_start_iter = poses->begin();
 100   CHECK(pose_start_iter != poses->end()) << "There are no poses.";
 101   problem->SetParameterBlockConstant(pose_start_iter->second.p.data());
 102   problem->SetParameterBlockConstant(pose_start_iter->second.q.coeffs().data());
 103 }
 104
 105 // Returns true if the solve was successful.
 106 bool SolveOptimizationProblem(ceres::Problem* problem) {
 107   CHECK(problem != NULL);
 108
 109   ceres::Solver::Options options;
 110   options.max_num_iterations = 200;
 111   options.linear_solver_type = ceres::SPARSE_NORMAL_CHOLESKY;
 112
 113   ceres::Solver::Summary summary;
 114   ceres::Solve(options, problem, &summary);
 115
 116   std::cout << summary.FullReport() << '\n';
 117
 118   return summary.IsSolutionUsable();
 119 }
 120
 121 // Output the poses to the file with format: id x y z q_x q_y q_z q_w.
 122 bool OutputPoses(const std::string& filename, const MapOfPoses& poses) {
 123   std::fstream outfile;
 124   outfile.open(filename.c_str(), std::istream::out);
 125   if (!outfile) {
 126     LOG(ERROR) << "Error opening the file: " << filename;
 127     return false;
 128   }
 129   for (std::map<int, Pose3d, std::less<int>,
 130                 Eigen::aligned_allocator<std::pair<const int, Pose3d> > >::
 131            const_iterator poses_iter = poses.begin();
 132        poses_iter != poses.end(); ++poses_iter) {
 133     const std::map<int, Pose3d, std::less<int>,
 134                    Eigen::aligned_allocator<std::pair<const int, Pose3d> > >::
 135         value_type& pair = *poses_iter;
 136     outfile << pair.first << " " << pair.second.p.transpose() << " "
 137             << pair.second.q.x() << " " << pair.second.q.y() << " "
 138             << pair.second.q.z() << " " << pair.second.q.w() << '\n';
 139   }
 140   return true;
 141 }
 142
 143 }  // namespace examples
 144 }  // namespace ceres
 145
 146 int main(int argc, char** argv) {
 147   google::InitGoogleLogging(argv[0]);
 148   CERES_GFLAGS_NAMESPACE::ParseCommandLineFlags(&argc, &argv, true);
 149
 150   CHECK(FLAGS_input != "") << "Need to specify the filename to read.";
 151
 152   ceres::examples::MapOfPoses poses;
 153   ceres::examples::VectorOfConstraints constraints;
 154
 155   CHECK(ceres::examples::ReadG2oFile(FLAGS_input, &poses, &constraints))
 156       << "Error reading the file: " << FLAGS_input;
 157
 158   std::cout << "Number of poses: " << poses.size() << '\n';
 159   std::cout << "Number of constraints: " << constraints.size() << '\n';
 160
 161   CHECK(ceres::examples::OutputPoses("poses_original.txt", poses))
 162       << "Error outputting to poses_original.txt";
 163
 164   ceres::Problem problem;
 165   ceres::examples::BuildOptimizationProblem(constraints, &poses, &problem);
 166
 167   CHECK(ceres::examples::SolveOptimizationProblem(&problem))
 168       << "The solve was not successful, exiting.";
 169
 170   CHECK(ceres::examples::OutputPoses("poses_optimized.txt", poses))
 171       << "Error outputting to poses_original.txt";
 172
 173   return 0;
 174 }