Add an example for modeling and solving a 2D pose graph SLAM problem. Change-Id: Ia89b12af7afa33e7b1b9a68d69cf2a0b53416737
diff --git a/docs/source/manhattan_olson_3500_result.png b/docs/source/manhattan_olson_3500_result.png new file mode 100644 index 0000000..5631dfa --- /dev/null +++ b/docs/source/manhattan_olson_3500_result.png Binary files differ
diff --git a/docs/source/nnls_tutorial.rst b/docs/source/nnls_tutorial.rst index 1ff2096..1c58646 100644 --- a/docs/source/nnls_tutorial.rst +++ b/docs/source/nnls_tutorial.rst
@@ -821,3 +821,136 @@ By fusing the noisy odometry and sensor readings this example demonstrates how to compute the maximum likelihood estimate (MLE) of the robot's pose at each timestep. + +#. `slam/pose_graph_2d/pose_graph_2d.cc + <https://ceres-solver.googlesource.com/ceres-solver/+/master/examples/slam/pose_graph_2d/pose_graph_2d.cc>`_ + The Simultaneous Localization and Mapping (SLAM) problem consists of building + a map of an unknown environment while simultaneously localizing against this + map. The main difficulty of this problem stems from not having any additional + external aiding information such as GPS. SLAM has been considered one of the + fundamental challenges of robotics. There are many resources on SLAM + [#f9]_. A pose graph optimization problem is one example of a SLAM + problem. The following explains how to formulate the pose graph based SLAM + problem in 2-Dimensions with relative pose constraints. + + Consider a robot moving in a 2-Dimensional plane. The robot has access to a + set of sensors such as wheel odometry or a laser range scanner. From these + raw measurements, we want to estimate the trajectory of the robot as well as + build a map of the environment. In order to reduce the computational + complexity of the problem, the pose graph approach abstracts the raw + measurements away. Specifically, it creates a graph of nodes which represent + the pose of the robot, and edges which represent the relative transformation + (delta position and orientation) between the two nodes. The edges are virtual + measurements derived from the raw sensor measurements, e.g. by integrating + the raw wheel odometry or aligning the laser range scans acquired from the + robot. A visualization of the resulting graph is shown below. + + .. figure:: slam2d.png + :figwidth: 500px + :height: 400px + :align: center + + Visual representation of a graph SLAM problem. + + The figure depicts the pose of the robot as the triangles, the measurements + are indicated by the connecting lines, and the loop closure measurements are + shown as dotted lines. Loop closures are measurements between non-sequential + robot states and they reduce the accumulation of error over time. The + following will describe the mathematical formulation of the pose graph + problem. + + The robot at timestamp :math:`t` has state :math:`x_t = [p^T, \psi]^T` where + :math:`p` is a 2D vector that represents the position in the plane and + :math:`\psi` is the orientation in radians. The measurement of the relative + transform between the robot state at two timestamps :math:`a` and :math:`b` + is given as: :math:`z_{ab} = [\hat{p}_{ab}^T, \hat{\psi}_{ab}]`. The residual + implemented in the Ceres cost function which computes the error between the + measurement and the predicted measurement is: + + .. math:: r_{ab} = + \left[ + \begin{array}{c} + R_a^T\left(p_b - p_a\right) - \hat{p}_{ab} \\ + \mathrm{Normalize}\left(\psi_b - \psi_a - \hat{\psi}_{ab}\right) + \end{array} + \right] + + where the function :math:`\mathrm{Normalize}()` normalizes the angle in the range + :math:`[-\pi,\pi)`, and :math:`R` is the rotation matrix given by + + .. math:: R_a = + \left[ + \begin{array}{cc} + \cos \psi_a & -\sin \psi_a \\ + \sin \psi_a & \cos \psi_a \\ + \end{array} + \right] + + To finish the cost function, we need to weight the residual by the + uncertainty of the measurement. Hence, we pre-multiply the residual by the + inverse square root of the covariance matrix for the measurement, + i.e. :math:`\Sigma_{ab}^{-\frac{1}{2}} r_{ab}` where :math:`\Sigma_{ab}` is + the covariance. + + Lastly, we use a local parameterization to normalize the orientation in the + range which is normalized between :math:`[-\pi,\pi)`. Specially, we define + the :member:`AngleLocalParameterization::operator()` function to be: + :math:`\mathrm{Normalize}(\psi + \delta \psi)`. + + This package includes an executable :member:`pose_graph_2d` that will read a + problem definition file. This executable can work with any 2D problem + definition that uses the g2o format. It would be relatively straightforward + to implement a new reader for a different format such as TORO or + others. :member:`pose_graph_2d` will print the Ceres solver full summary and + then output to disk the original and optimized poses (``poses_original.txt`` + and ``poses_optimized.txt``, respectively) of the robot in the following + format: + + .. code-block:: bash + + pose_id x y yaw_radians + pose_id x y yaw_radians + pose_id x y yaw_radians + + where ``pose_id`` is the corresponding integer ID from the file + definition. Note, the file will be sorted in ascending order for the + ``pose_id``. + + The executable :member:`pose_graph_2d` expects the first argument to be + the path to the problem definition. To run the executable, + + .. code-block:: bash + + /path/to/bin/pose_graph_2d /path/to/dataset/dataset.g2o + + where this assumes the install directory is located in the repository. + + A python script is provided to visualize the resulting output files. + + .. code-block:: bash + + /path/to/repo/robotics/slam/pose_graph_2d/plot_results.py --optimized_poses ./poses_optimized.txt --initial_poses ./poses_original.txt + + As an example, a standard synthetic benchmark dataset [#f10]_ created by Edwin + Olson which has 3500 nodes in a grid world with a total of 5598 edges was + solved. Visualizing the results with the provided script produces: + + .. figure:: manhattan_olson_3500_result.png + :figwidth: 600px + :height: 600px + :align: center + + with the original poses in green and the optimized poses in blue. As shown, + the optimized poses more closely match the underlying grid world. Note, the + left side of the graph has a small yaw drift due to a lack of relative + constraints to provide enough information to reconstruct the trajectory. + + .. rubric:: Footnotes + + .. [#f9] Giorgio Grisetti, Rainer Kummerle, Cyrill Stachniss, Wolfram + Burgard. A Tutorial on Graph-Based SLAM. IEEE Intelligent Transportation + Systems Magazine, 52(3):199–222, 2010. + + .. [#f10] E. Olson, J. Leonard, and S. Teller, “Fast iterative optimization of + pose graphs with poor initial estimates,” in Robotics and Automation + (ICRA), IEEE International Conference on, 2006, pp. 2262–2269.
diff --git a/docs/source/slam2d.png b/docs/source/slam2d.png new file mode 100644 index 0000000..ad287ee --- /dev/null +++ b/docs/source/slam2d.png Binary files differ
diff --git a/examples/CMakeLists.txt b/examples/CMakeLists.txt index 333a6a4..b589b45 100644 --- a/examples/CMakeLists.txt +++ b/examples/CMakeLists.txt
@@ -114,4 +114,5 @@ endif (GFLAGS) -add_subdirectory(sampled_function) \ No newline at end of file +add_subdirectory(sampled_function) +add_subdirectory(slam) \ No newline at end of file
diff --git a/examples/slam/CMakeLists.txt b/examples/slam/CMakeLists.txt new file mode 100644 index 0000000..d2c0d42 --- /dev/null +++ b/examples/slam/CMakeLists.txt
@@ -0,0 +1,30 @@ +# Ceres Solver - A fast non-linear least squares minimizer +# Copyright 2016 Google Inc. All rights reserved. +# http://ceres-solver.org/ +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions are met: +# +# * Redistributions of source code must retain the above copyright notice, +# this list of conditions and the following disclaimer. +# * Redistributions in binary form must reproduce the above copyright notice, +# this list of conditions and the following disclaimer in the documentation +# and/or other materials provided with the distribution. +# * Neither the name of Google Inc. nor the names of its contributors may be +# used to endorse or promote products derived from this software without +# specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +# Author: vitus@google.com (Michael Vitus) +add_subdirectory(pose_graph_2d)
diff --git a/examples/slam/pose_graph_2d/CMakeLists.txt b/examples/slam/pose_graph_2d/CMakeLists.txt new file mode 100644 index 0000000..117f4cd --- /dev/null +++ b/examples/slam/pose_graph_2d/CMakeLists.txt
@@ -0,0 +1,39 @@ +# Ceres Solver - A fast non-linear least squares minimizer +# Copyright 2016 Google Inc. All rights reserved. +# http://ceres-solver.org/ +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions are met: +# +# * Redistributions of source code must retain the above copyright notice, +# this list of conditions and the following disclaimer. +# * Redistributions in binary form must reproduce the above copyright notice, +# this list of conditions and the following disclaimer in the documentation +# and/or other materials provided with the distribution. +# * Neither the name of Google Inc. nor the names of its contributors may be +# used to endorse or promote products derived from this software without +# specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +# Author: vitus@google.com (Michael Vitus) + +add_executable(pose_graph_2d + angle_local_parameterization.h + normalize_angle.h + pose_graph_2d.cc + pose_graph_2d_error_term.h + read_g2o.cc + ) +target_link_libraries(pose_graph_2d ceres) +
diff --git a/examples/slam/pose_graph_2d/README.md b/examples/slam/pose_graph_2d/README.md new file mode 100644 index 0000000..ecac764 --- /dev/null +++ b/examples/slam/pose_graph_2d/README.md
@@ -0,0 +1,48 @@ +Pose Graph 2D +---------------- + +The Simultaneous Localization and Mapping (SLAM) problem consists of building a +map of an unknown environment while simultaneously localizing against this +map. The main difficulty of this problem stems from not having any additional +external aiding information such as GPS. SLAM has been considered one of the +fundamental challenges of robotics. A pose graph optimization problem is one +example of a SLAM problem. + +This package defines the necessary Ceres cost functions needed to model the +2-dimensional pose graph optimization problem as well as a binary to build and +solve the problem. The cost functions are shown for instruction purposes and can +be speed up by using analytical derivatives which take longer to implement. + +Running +----------- +This package includes an executable `pose_graph_2d` that will read a problem +definition file. This executable can work with any 2D problem definition that +uses the g2o format. It would be relatively straightforward to implement a new +reader for a different format such as toro or others. `pose_graph_2d` will print +the Ceres solver full summary and then output to disk the original and optimized +poses (`poses_original.txt` and `poses_optimized.txt`, respectively) of the +robot in the following format: + +``` +pose_id x y yaw_radians +pose_id x y yaw_radians +pose_id x y yaw_radians +... +``` + +where `pose_id` is the corresponding integer ID from the file definition. Note, +the file will be sorted in ascending order for the `pose_id`. + +The executable `solve_pose_graph_2d` expects the first argument to be the path +to the problem definition. To run the executable, + +``` +/path/to/bin/pose_graph_2d /path/to/dataset/dataset.g2o +``` + +where this assumes the install directory is located in the repository. + +A python script is provided to visualize the resulting output files. +``` +/path/to/repo/robotics/slam/pose_graph_2d/plot_results.py --optimized_poses ./poses_optimized.txt --initial_poses ./poses_original.txt +```
diff --git a/examples/slam/pose_graph_2d/angle_local_parameterization.h b/examples/slam/pose_graph_2d/angle_local_parameterization.h new file mode 100644 index 0000000..428cccc --- /dev/null +++ b/examples/slam/pose_graph_2d/angle_local_parameterization.h
@@ -0,0 +1,63 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2016 Google Inc. All rights reserved. +// http://ceres-solver.org/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: vitus@google.com (Michael Vitus) + +#ifndef CERES_EXAMPLES_POSE_GRAPH_2D_ANGLE_LOCAL_PARAMETERIZATION_H_ +#define CERES_EXAMPLES_POSE_GRAPH_2D_ANGLE_LOCAL_PARAMETERIZATION_H_ + +#include "ceres/local_parameterization.h" +#include "normalize_angle.h" + +namespace ceres { +namespace examples { + +// Defines a local parameterization for updating the angle to be constrained in +// [-pi to pi). +class AngleLocalParameterization { + public: + + template <typename T> + bool operator()(const T* theta_radians, const T* delta_theta_radians, + T* theta_radians_plus_delta) const { + *theta_radians_plus_delta = + NormalizeAngle(*theta_radians + *delta_theta_radians); + + return true; + } + + static ceres::LocalParameterization* Create() { + return (new ceres::AutoDiffLocalParameterization<AngleLocalParameterization, + 1, 1>); + } +}; + +} // namespace examples +} // namespace ceres + +#endif // CERES_EXAMPLES_POSE_GRAPH_2D_ANGLE_LOCAL_PARAMETERIZATION_H_
diff --git a/examples/slam/pose_graph_2d/normalize_angle.h b/examples/slam/pose_graph_2d/normalize_angle.h new file mode 100644 index 0000000..afd5df4 --- /dev/null +++ b/examples/slam/pose_graph_2d/normalize_angle.h
@@ -0,0 +1,53 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2016 Google Inc. All rights reserved. +// http://ceres-solver.org/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: vitus@google.com (Michael Vitus) + +#ifndef CERES_EXAMPLES_POSE_GRAPH_2D_NORMALIZE_ANGLE_H_ +#define CERES_EXAMPLES_POSE_GRAPH_2D_NORMALIZE_ANGLE_H_ + +#include <cmath> + +#include "ceres/ceres.h" + +namespace ceres { +namespace examples { + +// Normalizes the angle in radians between [-pi and pi). +template <typename T> +inline T NormalizeAngle(const T& angle_radians) { + // Use ceres::floor because it is specialized for double and Jet types. + T two_pi(2.0 * M_PI); + return angle_radians - + two_pi * ceres::floor((angle_radians + T(M_PI)) / two_pi); +} + +} // namespace examples +} // namespace ceres + +#endif // CERES_EXAMPLES_POSE_GRAPH_2D_NORMALIZE_ANGLE_H_
diff --git a/examples/slam/pose_graph_2d/plot_results.py b/examples/slam/pose_graph_2d/plot_results.py new file mode 100755 index 0000000..2c4d16e --- /dev/null +++ b/examples/slam/pose_graph_2d/plot_results.py
@@ -0,0 +1,45 @@ +#!/usr/bin/python +# +# Plots the results from the 2D pose graph optimization. It will draw a line +# between consecutive vertices. The commandline expects two optional filenames: +# +# ./plot_results.py --initial_poses optional --optimized_poses optional +# +# The files have the following format: +# ID x y yaw_radians + +import matplotlib.pyplot as plot +import numpy +import sys +from optparse import OptionParser + +parser = OptionParser() +parser.add_option("--initial_poses", dest="initial_poses", + default="", help="The filename that contains the original poses.") +parser.add_option("--optimized_poses", dest="optimized_poses", + default="", help="The filename that contains the optimized poses.") +(options, args) = parser.parse_args() + +# Read the original and optimized poses files. +poses_original = None +if options.initial_poses != '': + poses_original = numpy.genfromtxt(options.initial_poses, usecols = (1, 2)) + +poses_optimized = None +if options.optimized_poses != '': + poses_optimized = numpy.genfromtxt(options.optimized_poses, usecols = (1, 2)) + +# Plots the results for the specified poses. +plot.figure() +if poses_original is not None: + plot.plot(poses_original[:, 0], poses_original[:, 1], '-', label="Original", + alpha=0.5, color="green") + +if poses_optimized is not None: + plot.plot(poses_optimized[:, 0], poses_optimized[:, 1], '-', label="Optimized", + alpha=0.5, color="blue") + +plot.axis('equal') +plot.legend() +# Show the plot and wait for the user to close. +plot.show()
diff --git a/examples/slam/pose_graph_2d/pose_graph_2d.cc b/examples/slam/pose_graph_2d/pose_graph_2d.cc new file mode 100644 index 0000000..0310259 --- /dev/null +++ b/examples/slam/pose_graph_2d/pose_graph_2d.cc
@@ -0,0 +1,184 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2016 Google Inc. All rights reserved. +// http://ceres-solver.org/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: vitus@google.com (Michael Vitus) +// +// An example of solving a graph-based formulation of Simultaneous Localization +// and Mapping (SLAM). It reads a 2D pose graph problem definition file in the +// g2o format, formulates and solves the Ceres optimization problem, and outputs +// the original and optimized poses to file for plotting. + +#include <fstream> +#include <iostream> +#include <map> +#include <string> +#include <vector> + +#include "angle_local_parameterization.h" +#include "ceres/ceres.h" +#include "pose_graph_2d_error_term.h" +#include "read_g2o.h" +#include "types.h" + +namespace ceres { +namespace examples { + +// Constructs the nonlinear least squares optimization problem from the pose +// graph constraints and solves it. +void BuildOptimizationProblem(const std::vector<Constraint2d>& constraints, + std::map<int, Pose2d>* poses, + ceres::Problem* problem) { + CHECK(poses != NULL); + CHECK(problem != NULL); + if (constraints.empty()) { + std::cout << "No constraints, no problem to optimize.\n"; + return; + } + + ceres::LossFunction* loss_function = NULL; + ceres::LocalParameterization* angle_local_parameterization = + AngleLocalParameterization::Create(); + + for (std::vector<Constraint2d>::const_iterator constraints_iter = + constraints.begin(); + constraints_iter != constraints.end(); ++constraints_iter) { + const Constraint2d& constraint = *constraints_iter; + + std::map<int, Pose2d>::iterator pose_begin_iter = + poses->find(constraint.id_begin); + CHECK(pose_begin_iter != poses->end()) + << "Pose with ID: " << constraint.id_begin << " not found."; + std::map<int, Pose2d>::iterator pose_end_iter = + poses->find(constraint.id_end); + CHECK(pose_end_iter != poses->end()) + << "Pose with ID: " << constraint.id_end << " not found."; + + const Eigen::Matrix3d sqrt_information = + constraint.information.llt().matrixL(); + // Ceres will take ownership of the pointer. + ceres::CostFunction* cost_function = PoseGraph2dErrorTerm::Create( + constraint.x, constraint.y, constraint.yaw_radians, sqrt_information); + problem->AddResidualBlock( + cost_function, loss_function, &pose_begin_iter->second.x, + &pose_begin_iter->second.y, &pose_begin_iter->second.yaw_radians, + &pose_end_iter->second.x, &pose_end_iter->second.y, + &pose_end_iter->second.yaw_radians); + + problem->SetParameterization(&pose_begin_iter->second.yaw_radians, + angle_local_parameterization); + problem->SetParameterization(&pose_end_iter->second.yaw_radians, + angle_local_parameterization); + } + + // The pose graph optimization problem has three DOFs that are not fully + // constrained. This is typically referred to as gauge freedom. You can apply + // a rigid body transformation to all the nodes and the optimization problem + // will still have the exact same cost. The Levenberg-Marquardt algorithm has + // internal damping which mitigate this issue, but it is better to properly + // constrain the gauge freedom. This can be done by setting one of the poses + // as constant so the optimizer cannot change it. + std::map<int, Pose2d>::iterator pose_start_iter = + poses->begin(); + CHECK(pose_start_iter != poses->end()) << "There are no poses."; + problem->SetParameterBlockConstant(&pose_start_iter->second.x); + problem->SetParameterBlockConstant(&pose_start_iter->second.y); + problem->SetParameterBlockConstant(&pose_start_iter->second.yaw_radians); +} + +// Returns true if the solve was successful. +bool SolveOptimizationProblem(ceres::Problem* problem) { + CHECK(problem != NULL); + + ceres::Solver::Options options; + options.max_num_iterations = 100; + options.linear_solver_type = ceres::SPARSE_NORMAL_CHOLESKY; + + ceres::Solver::Summary summary; + ceres::Solve(options, problem, &summary); + + std::cout << summary.FullReport() << '\n'; + + return summary.IsSolutionUsable(); +} + +// Output the poses to the file with format: ID x y yaw_radians. +bool OutputPoses(const std::string& filename, + const std::map<int, Pose2d>& poses) { + std::fstream outfile; + outfile.open(filename.c_str(), std::istream::out); + if (!outfile) { + std::cerr << "Error opening the file: " << filename << '\n'; + return false; + } + for (std::map<int, Pose2d>::const_iterator poses_iter = poses.begin(); + poses_iter != poses.end(); ++poses_iter) { + const std::map<int, Pose2d>::value_type& pair = *poses_iter; + outfile << pair.first << " " << pair.second.x << " " << pair.second.y + << ' ' << pair.second.yaw_radians << '\n'; + } + return true; +} + +} // namespace examples +} // namespace ceres + +int main(int argc, char** argv) { + if (argc != 2) { + std::cerr << "Need to specify the filename to read as the first and only " + << "argument.\n"; + return -1; + } + + std::map<int, ceres::examples::Pose2d> poses; + std::vector<ceres::examples::Constraint2d> constraints; + + if (!ceres::examples::ReadG2oFile(argv[1], &poses, &constraints)) { + return -1; + } + + std::cout << "Number of poses: " << poses.size() << '\n'; + std::cout << "Number of constraints: " << constraints.size() << '\n'; + + if (!ceres::examples::OutputPoses("poses_original.txt", poses)) { + return -1; + } + + ceres::Problem problem; + ceres::examples::BuildOptimizationProblem(constraints, &poses, &problem); + + if (!ceres::examples::SolveOptimizationProblem(&problem)) { + std::cout << "The solve was not successful, exiting.\n"; + return -1; + } + + if (!ceres::examples::OutputPoses("poses_optimized.txt", poses)) { + return -1; + } + + return 0; +}
diff --git a/examples/slam/pose_graph_2d/pose_graph_2d_error_term.h b/examples/slam/pose_graph_2d/pose_graph_2d_error_term.h new file mode 100644 index 0000000..20656d2 --- /dev/null +++ b/examples/slam/pose_graph_2d/pose_graph_2d_error_term.h
@@ -0,0 +1,112 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2016 Google Inc. All rights reserved. +// http://ceres-solver.org/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: vitus@google.com (Michael Vitus) +// +// Cost function for a 2D pose graph formulation. + +#ifndef CERES_EXAMPLES_POSE_GRAPH_2D_POSE_GRAPH_2D_ERROR_TERM_H_ +#define CERES_EXAMPLES_POSE_GRAPH_2D_POSE_GRAPH_2D_ERROR_TERM_H_ + +#include "Eigen/Core" + +namespace ceres { +namespace examples { + +template <typename T> +Eigen::Matrix<T, 2, 2> RotationMatrix2D(T yaw_radians) { + const T cos_yaw = ceres::cos(yaw_radians); + const T sin_yaw = ceres::sin(yaw_radians); + + Eigen::Matrix<T, 2, 2> rotation; + rotation << cos_yaw, -sin_yaw, sin_yaw, cos_yaw; + return rotation; +} + +// Computes the error term for two poses that have a relative pose measurement +// between them. Let the hat variables be the measurement. +// +// residual = information^{1/2} * [ r_a^T * (p_b - p_a) - \hat{p_ab} ] +// [ Normalize(yaw_b - yaw_a - \hat{yaw_ab}) ] +// +// where r_a is the rotation matrix that rotates a vector represented in frame A +// into the global frame, and Normalize(*) ensures the angles are in the range +// [-pi, pi). +class PoseGraph2dErrorTerm { + public: + PoseGraph2dErrorTerm(double x_ab, double y_ab, double yaw_ab_radians, + const Eigen::Matrix3d& sqrt_information) + : p_ab_(x_ab, y_ab), + yaw_ab_radians_(yaw_ab_radians), + sqrt_information_(sqrt_information) {} + + template <typename T> + bool operator()(const T* const x_a, const T* const y_a, const T* const yaw_a, + const T* const x_b, const T* const y_b, const T* const yaw_b, + T* residuals_ptr) const { + const Eigen::Matrix<T, 2, 1> p_a(*x_a, *y_a); + const Eigen::Matrix<T, 2, 1> p_b(*x_b, *y_b); + + Eigen::Map<Eigen::Matrix<T, 3, 1> > residuals_map(residuals_ptr); + + residuals_map.template head<2>() = + RotationMatrix2D(*yaw_a).transpose() * (p_b - p_a) - + p_ab_.cast<T>(); + residuals_map(2) = ceres::examples::NormalizeAngle( + (*yaw_b - *yaw_a) - static_cast<T>(yaw_ab_radians_)); + + // Scale the residuals by the square root information matrix to account for + // the measurement uncertainty. + residuals_map = sqrt_information_.template cast<T>() * residuals_map; + + return true; + } + + static ceres::CostFunction* Create(double x_ab, double y_ab, + double yaw_ab_radians, + const Eigen::Matrix3d& sqrt_information) { + return (new ceres::AutoDiffCostFunction<PoseGraph2dErrorTerm, 3, 1, 1, 1, 1, + 1, 1>(new PoseGraph2dErrorTerm( + x_ab, y_ab, yaw_ab_radians, sqrt_information))); + } + + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + + private: + // The position of B relative to A in the A frame. + const Eigen::Vector2d p_ab_; + // The orientation of frame B relative to frame A. + const double yaw_ab_radians_; + // The inverse square root of the measurement covariance matrix. + const Eigen::Matrix3d sqrt_information_; +}; + +} // namespace examples +} // namespace ceres + +#endif // CERES_EXAMPLES_POSE_GRAPH_2D_POSE_GRAPH_2D_ERROR_TERM_H_
diff --git a/examples/slam/pose_graph_2d/read_g2o.cc b/examples/slam/pose_graph_2d/read_g2o.cc new file mode 100644 index 0000000..22f424b --- /dev/null +++ b/examples/slam/pose_graph_2d/read_g2o.cc
@@ -0,0 +1,122 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2016 Google Inc. All rights reserved. +// http://ceres-solver.org/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: vitus@google.com (Michael Vitus) + +#include "read_g2o.h" + +#include <iostream> +#include <fstream> + +#include "Eigen/Core" +#include "glog/logging.h" +#include "normalize_angle.h" + +namespace ceres { +namespace examples { +namespace { +// Reads a single pose from the input and inserts it into the map. Returns false +// if there is a duplicate entry. +bool ReadVertex(std::ifstream* infile, std::map<int, Pose2d>* poses) { + int id; + Pose2d pose; + *infile >> id >> pose.x >> pose.y >> pose.yaw_radians; + // Normalize the angle between -pi to pi. + pose.yaw_radians = NormalizeAngle(pose.yaw_radians); + // Ensure we don't have duplicate poses. + if (poses->find(id) != poses->end()) { + std::cerr << "Duplicate vertex with ID: " << id << '\n'; + return false; + } + (*poses)[id] = pose; + return true; +} + +// Reads the contraints between two vertices in the pose graph +void ReadConstraint(std::ifstream* infile, + std::vector<Constraint2d>* constraints) { + Constraint2d constraint; + + // Read in the constraint data which is the x, y, yaw_radians and then the + // upper triangular part of the information matrix. + *infile >> constraint.id_begin >> constraint.id_end >> constraint.x >> + constraint.y >> constraint.yaw_radians >> + constraint.information(0, 0) >> constraint.information(0, 1) >> + constraint.information(0, 2) >> constraint.information(1, 1) >> + constraint.information(1, 2) >> constraint.information(2, 2); + + // Set the lower triangular part of the information matrix. + constraint.information(1, 0) = constraint.information(0, 1); + constraint.information(2, 0) = constraint.information(0, 2); + constraint.information(2, 1) = constraint.information(1, 2); + + // Normalize the angle between -pi to pi. + constraint.yaw_radians = NormalizeAngle(constraint.yaw_radians); + + constraints->push_back(constraint); +} +} + +bool ReadG2oFile(const std::string& filename, std::map<int, Pose2d>* poses, + std::vector<Constraint2d>* constraints) { + CHECK(poses != NULL); + CHECK(constraints != NULL); + + poses->clear(); + constraints->clear(); + + std::ifstream infile(filename.c_str()); + if (!infile) { + std::cerr << "Error reading the file: " << filename << '\n'; + return false; + } + + std::string data_type; + while (infile.good()) { + // Read whether the type is a node or a constraint. + infile >> data_type; + if (data_type == "VERTEX_SE2") { + if (!ReadVertex(&infile, poses)) { + return false; + } + } else if (data_type == "EDGE_SE2") { + ReadConstraint(&infile, constraints); + } else { + std::cerr << "Unknown data type: " << data_type << '\n'; + return false; + } + + // Clear any trailing whitespace from the file. + infile >> std::ws; + } + + return true; +} + +} // namespace examples +} // namespace ceres
diff --git a/examples/slam/pose_graph_2d/read_g2o.h b/examples/slam/pose_graph_2d/read_g2o.h new file mode 100644 index 0000000..dfa30d6 --- /dev/null +++ b/examples/slam/pose_graph_2d/read_g2o.h
@@ -0,0 +1,64 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2016 Google Inc. All rights reserved. +// http://ceres-solver.org/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: vitus@google.com (Michael Vitus) +// +// Reads a 2D pose graph problem formulation in g2o format. + +#ifndef CERES_EXAMPLES_POSE_GRAPH_2D_READ_G2O_H_ +#define CERES_EXAMPLES_POSE_GRAPH_2D_READ_G2O_H_ + +#include <string> +#include <map> +#include <vector> + +#include "types.h" + +namespace ceres { +namespace examples { + +// Reads a file in the g2o filename format that describes a 2D pose graph +// problem. The g2o format consists of two entries, vertices and constraints. A +// vertex is defined as follows: +// +// VERTEX_SE2 ID x_meters y_meters yaw_radians +// +// A constraint is defined as follows: +// +// EDGE_SE2 ID_A ID_B A_x_B A_y_B A_yaw_B I_11 I_12 I_13 I_22 I_23 I_33 +// +// where I_ij is the (i, j)-th entry of the information matrix for the +// measurement. +bool ReadG2oFile(const std::string& filename, + std::map<int, Pose2d>* poses, + std::vector<Constraint2d>* constraints); + +} // namespace examples +} // namespace ceres + +#endif // CERES_EXAMPLES_POSE_GRAPH_2D_READ_G2O_H_
diff --git a/examples/slam/pose_graph_2d/types.h b/examples/slam/pose_graph_2d/types.h new file mode 100644 index 0000000..d54c733 --- /dev/null +++ b/examples/slam/pose_graph_2d/types.h
@@ -0,0 +1,68 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2016 Google Inc. All rights reserved. +// http://ceres-solver.org/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: vitus@google.com (Michael Vitus) +// +// Defines the types used in the 2D pose graph SLAM formulation. Each vertex of +// the graph has a unique integer ID with a position and orientation. There are +// delta transformation constraints between two vertices. + +#ifndef CERES_EXAMPLES_POSE_GRAPH_2D_TYPES_H_ +#define CERES_EXAMPLES_POSE_GRAPH_2D_TYPES_H_ + +#include "Eigen/Core" + +namespace ceres { +namespace examples { + +// The state for each vertex in the pose graph. +struct Pose2d { + double x; + double y; + double yaw_radians; +}; + +// The constraint between two vertices in the pose graph. The constraint is the +// transformation from vertex id_begin to vertex id_end. +struct Constraint2d { + int id_begin; + int id_end; + + double x; + double y; + double yaw_radians; + + // The inverse of the covariance matrix for the measurement. The order of the + // entries are x, y, and yaw. + Eigen::Matrix3d information; +}; + +} // namespace examples +} // namespace ceres + +#endif // CERES_EXAMPLES_POSE_GRAPH_2D_TYPES_H_
