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_
