Add an example for modeling and solving a 2D pose graph SLAM problem.

Change-Id: Ia89b12af7afa33e7b1b9a68d69cf2a0b53416737

docs/source/nnls_tutorial.rst

@@ -821,3 +821,136 @@ directory contains a number of other examples:
    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.

examples/CMakeLists.txt

@@ -114,4 +114,5 @@ if (GFLAGS)
 
 endif (GFLAGS)
 
-add_subdirectory(sampled_function)
+add_subdirectory(sampled_function)
+add_subdirectory(slam)

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)

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)
+

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
+```

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_

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_

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()

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;
+}

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_

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

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_

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_