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