Squashed 'third_party/ceres/' content from commit e51e9b4
Change-Id: I763587619d57e594d3fa158dc3a7fe0b89a1743b
git-subtree-dir: third_party/ceres
git-subtree-split: e51e9b46f6ca88ab8b2266d0e362771db6d98067
diff --git a/examples/slam/CMakeLists.txt b/examples/slam/CMakeLists.txt
new file mode 100644
index 0000000..c72aa16
--- /dev/null
+++ b/examples/slam/CMakeLists.txt
@@ -0,0 +1,33 @@
+# 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_directories(./)
+
+add_subdirectory(pose_graph_2d)
+add_subdirectory(pose_graph_3d)
diff --git a/examples/slam/common/read_g2o.h b/examples/slam/common/read_g2o.h
new file mode 100644
index 0000000..71b071c
--- /dev/null
+++ b/examples/slam/common/read_g2o.h
@@ -0,0 +1,141 @@
+// 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 file in the g2o filename format that describes a pose graph problem.
+
+#ifndef EXAMPLES_CERES_READ_G2O_H_
+#define EXAMPLES_CERES_READ_G2O_H_
+
+#include <fstream>
+#include <string>
+
+#include "glog/logging.h"
+
+namespace ceres {
+namespace examples {
+
+// Reads a single pose from the input and inserts it into the map. Returns false
+// if there is a duplicate entry.
+template <typename Pose, typename Allocator>
+bool ReadVertex(std::ifstream* infile,
+ std::map<int, Pose, std::less<int>, Allocator>* poses) {
+ int id;
+ Pose pose;
+ *infile >> id >> pose;
+
+ // Ensure we don't have duplicate poses.
+ if (poses->find(id) != poses->end()) {
+ LOG(ERROR) << "Duplicate vertex with ID: " << id;
+ return false;
+ }
+ (*poses)[id] = pose;
+
+ return true;
+}
+
+// Reads the contraints between two vertices in the pose graph
+template <typename Constraint, typename Allocator>
+void ReadConstraint(std::ifstream* infile,
+ std::vector<Constraint, Allocator>* constraints) {
+ Constraint constraint;
+ *infile >> constraint;
+
+ constraints->push_back(constraint);
+}
+
+// Reads a file in the g2o filename format that describes a pose graph
+// problem. The g2o format consists of two entries, vertices and constraints.
+//
+// In 2D, 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.
+//
+//
+// In 3D, a vertex is defined as follows:
+//
+// VERTEX_SE3:QUAT ID x y z q_x q_y q_z q_w
+//
+// where the quaternion is in Hamilton form.
+// A constraint is defined as follows:
+//
+// EDGE_SE3:QUAT ID_a ID_b x_ab y_ab z_ab q_x_ab q_y_ab q_z_ab q_w_ab I_11 I_12 I_13 ... I_16 I_22 I_23 ... I_26 ... I_66 // NOLINT
+//
+// where I_ij is the (i, j)-th entry of the information matrix for the
+// measurement. Only the upper-triangular part is stored. The measurement order
+// is the delta position followed by the delta orientation.
+template <typename Pose, typename Constraint, typename MapAllocator,
+ typename VectorAllocator>
+bool ReadG2oFile(const std::string& filename,
+ std::map<int, Pose, std::less<int>, MapAllocator>* poses,
+ std::vector<Constraint, VectorAllocator>* constraints) {
+ CHECK(poses != NULL);
+ CHECK(constraints != NULL);
+
+ poses->clear();
+ constraints->clear();
+
+ std::ifstream infile(filename.c_str());
+ if (!infile) {
+ 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 == Pose::name()) {
+ if (!ReadVertex(&infile, poses)) {
+ return false;
+ }
+ } else if (data_type == Constraint::name()) {
+ ReadConstraint(&infile, constraints);
+ } else {
+ LOG(ERROR) << "Unknown data type: " << data_type;
+ return false;
+ }
+
+ // Clear any trailing whitespace from the line.
+ infile >> std::ws;
+ }
+
+ return true;
+}
+
+} // namespace examples
+} // namespace ceres
+
+#endif // EXAMPLES_CERES_READ_G2O_H_
diff --git a/examples/slam/pose_graph_2d/CMakeLists.txt b/examples/slam/pose_graph_2d/CMakeLists.txt
new file mode 100644
index 0000000..d654e8c
--- /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)
+
+if (GFLAGS)
+ add_executable(pose_graph_2d
+ angle_local_parameterization.h
+ normalize_angle.h
+ pose_graph_2d.cc
+ pose_graph_2d_error_term.h
+ types.h)
+ target_link_libraries(pose_graph_2d ceres ${GFLAGS_LIBRARIES})
+endif (GFLAGS)
\ No newline at end of file
diff --git a/examples/slam/pose_graph_2d/README.md b/examples/slam/pose_graph_2d/README.md
new file mode 100644
index 0000000..b5ff987
--- /dev/null
+++ b/examples/slam/pose_graph_2d/README.md
@@ -0,0 +1,46 @@
+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 `pose_graph_2d` has one flag `--input` which is the path to the
+problem definition. To run the executable,
+
+```
+/path/to/bin/pose_graph_2d --input /path/to/dataset/dataset.g2o
+```
+
+A python script is provided to visualize the resulting output files.
+```
+/path/to/repo/examples/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..b9374db
--- /dev/null
+++ b/examples/slam/pose_graph_2d/pose_graph_2d.cc
@@ -0,0 +1,182 @@
+// 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 "common/read_g2o.h"
+#include "gflags/gflags.h"
+#include "glog/logging.h"
+#include "pose_graph_2d_error_term.h"
+#include "types.h"
+
+DEFINE_string(input, "", "The pose graph definition filename in g2o format.");
+
+namespace ceres {
+namespace examples {
+
+// Constructs the nonlinear least squares optimization problem from the pose
+// graph constraints.
+void BuildOptimizationProblem(const std::vector<Constraint2d>& constraints,
+ std::map<int, Pose2d>* poses,
+ ceres::Problem* problem) {
+ CHECK(poses != NULL);
+ CHECK(problem != NULL);
+ if (constraints.empty()) {
+ LOG(INFO) << "No constraints, no problem to optimize.";
+ 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) {
+ google::InitGoogleLogging(argv[0]);
+ CERES_GFLAGS_NAMESPACE::ParseCommandLineFlags(&argc, &argv, true);
+
+ CHECK(FLAGS_input != "") << "Need to specify the filename to read.";
+
+ std::map<int, ceres::examples::Pose2d> poses;
+ std::vector<ceres::examples::Constraint2d> constraints;
+
+ CHECK(ceres::examples::ReadG2oFile(FLAGS_input, &poses, &constraints))
+ << "Error reading the file: " << FLAGS_input;
+
+ std::cout << "Number of poses: " << poses.size() << '\n';
+ std::cout << "Number of constraints: " << constraints.size() << '\n';
+
+ CHECK(ceres::examples::OutputPoses("poses_original.txt", poses))
+ << "Error outputting to poses_original.txt";
+
+ ceres::Problem problem;
+ ceres::examples::BuildOptimizationProblem(constraints, &poses, &problem);
+
+ CHECK(ceres::examples::SolveOptimizationProblem(&problem))
+ << "The solve was not successful, exiting.";
+
+ CHECK(ceres::examples::OutputPoses("poses_optimized.txt", poses))
+ << "Error outputting to poses_original.txt";
+
+ 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/types.h b/examples/slam/pose_graph_2d/types.h
new file mode 100644
index 0000000..a54d9bf
--- /dev/null
+++ b/examples/slam/pose_graph_2d/types.h
@@ -0,0 +1,105 @@
+// 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 <fstream>
+
+#include "Eigen/Core"
+#include "normalize_angle.h"
+
+namespace ceres {
+namespace examples {
+
+// The state for each vertex in the pose graph.
+struct Pose2d {
+ double x;
+ double y;
+ double yaw_radians;
+
+ // The name of the data type in the g2o file format.
+ static std::string name() {
+ return "VERTEX_SE2";
+ }
+};
+
+std::istream& operator>>(std::istream& input, Pose2d& pose) {
+ input >> pose.x >> pose.y >> pose.yaw_radians;
+ // Normalize the angle between -pi to pi.
+ pose.yaw_radians = NormalizeAngle(pose.yaw_radians);
+ return input;
+}
+
+// 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;
+
+ // The name of the data type in the g2o file format.
+ static std::string name() {
+ return "EDGE_SE2";
+ }
+};
+
+std::istream& operator>>(std::istream& input, Constraint2d& constraint) {
+ input >> 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);
+ return input;
+}
+
+} // namespace examples
+} // namespace ceres
+
+#endif // CERES_EXAMPLES_POSE_GRAPH_2D_TYPES_H_
diff --git a/examples/slam/pose_graph_3d/CMakeLists.txt b/examples/slam/pose_graph_3d/CMakeLists.txt
new file mode 100644
index 0000000..2c5fdc3
--- /dev/null
+++ b/examples/slam/pose_graph_3d/CMakeLists.txt
@@ -0,0 +1,34 @@
+# 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)
+
+if (GFLAGS)
+ add_executable(pose_graph_3d pose_graph_3d.cc)
+ target_link_libraries(pose_graph_3d ceres ${GFLAGS_LIBRARIES})
+endif (GFLAGS)
\ No newline at end of file
diff --git a/examples/slam/pose_graph_3d/README.md b/examples/slam/pose_graph_3d/README.md
new file mode 100644
index 0000000..badc1ec
--- /dev/null
+++ b/examples/slam/pose_graph_3d/README.md
@@ -0,0 +1,54 @@
+Pose Graph 3D
+----------------
+
+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.
+
+The example also illustrates how to use Eigen's geometry module with Ceres'
+automatic differentiation functionality. To represent the orientation, we will
+use Eigen's quaternion which uses the Hamiltonian convention but has different
+element ordering as compared with Ceres's rotation representation. Specifically
+they differ by whether the scalar component q_w is first or last; the element
+order for Ceres's quaternion is [q_w, q_x, q_y, q_z] where as Eigen's quaternion
+is [q_x, q_y, q_z, q_w].
+
+This package defines the necessary Ceres cost functions needed to model the
+3-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_3d` that will read a problem
+definition file. This executable can work with any 3D problem definition that
+uses the g2o format with quaternions used for the orientation representation. It
+would be relatively straightforward to implement a new reader for a different
+format such as TORO or others. `pose_graph_3d` 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 z q_x q_y q_z q_w
+pose_id x y z q_x q_y q_z q_w
+pose_id x y z q_x q_y q_z q_w
+...
+```
+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 `pose_graph_3d` has one flag `--input` which is the path to the
+problem definition. To run the executable,
+```
+/path/to/bin/pose_graph_3d --input /path/to/dataset/dataset.g2o
+```
+
+A script is provided to visualize the resulting output files. There is also an
+option to enable equal axes using ```--axes_equal```.
+```
+/path/to/repo/examples/slam/pose_graph_3d/plot_results.py --optimized_poses ./poses_optimized.txt --initial_poses ./poses_original.txt
+```
diff --git a/examples/slam/pose_graph_3d/plot_results.py b/examples/slam/pose_graph_3d/plot_results.py
new file mode 100755
index 0000000..defb9e6
--- /dev/null
+++ b/examples/slam/pose_graph_3d/plot_results.py
@@ -0,0 +1,80 @@
+#!/usr/bin/python
+#
+# Plots the results from the 3D pose graph optimization. It will draw a line
+# between consecutive vertices. The commandline expects two optional filenames:
+#
+# ./plot_results.py --initial_poses filename --optimized_poses filename
+#
+# The files have the following format:
+# ID x y z q_x q_y q_z q_w
+
+from mpl_toolkits.mplot3d import Axes3D
+import matplotlib.pyplot as plot
+import numpy
+import sys
+from optparse import OptionParser
+
+def set_axes_equal(axes):
+ ''' Sets the axes of a 3D plot to have equal scale. '''
+ x_limits = axes.get_xlim3d()
+ y_limits = axes.get_ylim3d()
+ z_limits = axes.get_zlim3d()
+
+ x_range = abs(x_limits[1] - x_limits[0])
+ x_middle = numpy.mean(x_limits)
+ y_range = abs(y_limits[1] - y_limits[0])
+ y_middle = numpy.mean(y_limits)
+ z_range = abs(z_limits[1] - z_limits[0])
+ z_middle = numpy.mean(z_limits)
+
+ length = 0.5 * max([x_range, y_range, z_range])
+
+ axes.set_xlim3d([x_middle - length, x_middle + length])
+ axes.set_ylim3d([y_middle - length, y_middle + length])
+ axes.set_zlim3d([z_middle - length, z_middle + length])
+
+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.")
+parser.add_option("-e", "--axes_equal", action="store_true", dest="axes_equal",
+ default="", help="Make the plot axes equal.")
+(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, 3))
+
+poses_optimized = None
+if options.optimized_poses != '':
+ poses_optimized = numpy.genfromtxt(options.optimized_poses,
+ usecols = (1, 2, 3))
+
+# Plots the results for the specified poses.
+figure = plot.figure()
+
+if poses_original is not None:
+ axes = plot.subplot(1, 2, 1, projection='3d')
+ plot.plot(poses_original[:, 0], poses_original[:, 1], poses_original[:, 2],
+ '-', alpha=0.5, color="green")
+ plot.title('Original')
+ if options.axes_equal:
+ axes.set_aspect('equal')
+ set_axes_equal(axes)
+
+
+if poses_optimized is not None:
+ axes = plot.subplot(1, 2, 2, projection='3d')
+ plot.plot(poses_optimized[:, 0], poses_optimized[:, 1], poses_optimized[:, 2],
+ '-', alpha=0.5, color="blue")
+ plot.title('Optimized')
+ if options.axes_equal:
+ axes.set_aspect('equal')
+ set_axes_equal(plot.gca())
+
+
+# Show the plot and wait for the user to close.
+plot.show()
diff --git a/examples/slam/pose_graph_3d/pose_graph_3d.cc b/examples/slam/pose_graph_3d/pose_graph_3d.cc
new file mode 100644
index 0000000..dcc85af
--- /dev/null
+++ b/examples/slam/pose_graph_3d/pose_graph_3d.cc
@@ -0,0 +1,174 @@
+// 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 <iostream>
+#include <fstream>
+#include <string>
+
+#include "ceres/ceres.h"
+#include "common/read_g2o.h"
+#include "gflags/gflags.h"
+#include "glog/logging.h"
+#include "pose_graph_3d_error_term.h"
+#include "types.h"
+
+DEFINE_string(input, "", "The pose graph definition filename in g2o format.");
+
+namespace ceres {
+namespace examples {
+
+// Constructs the nonlinear least squares optimization problem from the pose
+// graph constraints.
+void BuildOptimizationProblem(const VectorOfConstraints& constraints,
+ MapOfPoses* poses, ceres::Problem* problem) {
+ CHECK(poses != NULL);
+ CHECK(problem != NULL);
+ if (constraints.empty()) {
+ LOG(INFO) << "No constraints, no problem to optimize.";
+ return;
+ }
+
+ ceres::LossFunction* loss_function = NULL;
+ ceres::LocalParameterization* quaternion_local_parameterization =
+ new EigenQuaternionParameterization;
+
+ for (VectorOfConstraints::const_iterator constraints_iter =
+ constraints.begin();
+ constraints_iter != constraints.end(); ++constraints_iter) {
+ const Constraint3d& constraint = *constraints_iter;
+
+ MapOfPoses::iterator pose_begin_iter = poses->find(constraint.id_begin);
+ CHECK(pose_begin_iter != poses->end())
+ << "Pose with ID: " << constraint.id_begin << " not found.";
+ MapOfPoses::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::Matrix<double, 6, 6> sqrt_information =
+ constraint.information.llt().matrixL();
+ // Ceres will take ownership of the pointer.
+ ceres::CostFunction* cost_function =
+ PoseGraph3dErrorTerm::Create(constraint.t_be, sqrt_information);
+
+ problem->AddResidualBlock(cost_function, loss_function,
+ pose_begin_iter->second.p.data(),
+ pose_begin_iter->second.q.coeffs().data(),
+ pose_end_iter->second.p.data(),
+ pose_end_iter->second.q.coeffs().data());
+
+ problem->SetParameterization(pose_begin_iter->second.q.coeffs().data(),
+ quaternion_local_parameterization);
+ problem->SetParameterization(pose_end_iter->second.q.coeffs().data(),
+ quaternion_local_parameterization);
+ }
+
+ // The pose graph optimization problem has six 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 mitigates 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.
+ MapOfPoses::iterator pose_start_iter = poses->begin();
+ CHECK(pose_start_iter != poses->end()) << "There are no poses.";
+ problem->SetParameterBlockConstant(pose_start_iter->second.p.data());
+ problem->SetParameterBlockConstant(pose_start_iter->second.q.coeffs().data());
+}
+
+// Returns true if the solve was successful.
+bool SolveOptimizationProblem(ceres::Problem* problem) {
+ CHECK(problem != NULL);
+
+ ceres::Solver::Options options;
+ options.max_num_iterations = 200;
+ 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 z q_x q_y q_z q_w.
+bool OutputPoses(const std::string& filename, const MapOfPoses& poses) {
+ std::fstream outfile;
+ outfile.open(filename.c_str(), std::istream::out);
+ if (!outfile) {
+ LOG(ERROR) << "Error opening the file: " << filename;
+ return false;
+ }
+ for (std::map<int, Pose3d, std::less<int>,
+ Eigen::aligned_allocator<std::pair<const int, Pose3d> > >::
+ const_iterator poses_iter = poses.begin();
+ poses_iter != poses.end(); ++poses_iter) {
+ const std::map<int, Pose3d, std::less<int>,
+ Eigen::aligned_allocator<std::pair<const int, Pose3d> > >::
+ value_type& pair = *poses_iter;
+ outfile << pair.first << " " << pair.second.p.transpose() << " "
+ << pair.second.q.x() << " " << pair.second.q.y() << " "
+ << pair.second.q.z() << " " << pair.second.q.w() << '\n';
+ }
+ return true;
+}
+
+} // namespace examples
+} // namespace ceres
+
+int main(int argc, char** argv) {
+ google::InitGoogleLogging(argv[0]);
+ CERES_GFLAGS_NAMESPACE::ParseCommandLineFlags(&argc, &argv, true);
+
+ CHECK(FLAGS_input != "") << "Need to specify the filename to read.";
+
+ ceres::examples::MapOfPoses poses;
+ ceres::examples::VectorOfConstraints constraints;
+
+ CHECK(ceres::examples::ReadG2oFile(FLAGS_input, &poses, &constraints))
+ << "Error reading the file: " << FLAGS_input;
+
+ std::cout << "Number of poses: " << poses.size() << '\n';
+ std::cout << "Number of constraints: " << constraints.size() << '\n';
+
+ CHECK(ceres::examples::OutputPoses("poses_original.txt", poses))
+ << "Error outputting to poses_original.txt";
+
+ ceres::Problem problem;
+ ceres::examples::BuildOptimizationProblem(constraints, &poses, &problem);
+
+ CHECK(ceres::examples::SolveOptimizationProblem(&problem))
+ << "The solve was not successful, exiting.";
+
+ CHECK(ceres::examples::OutputPoses("poses_optimized.txt", poses))
+ << "Error outputting to poses_original.txt";
+
+ return 0;
+}
diff --git a/examples/slam/pose_graph_3d/pose_graph_3d_error_term.h b/examples/slam/pose_graph_3d/pose_graph_3d_error_term.h
new file mode 100644
index 0000000..aca819e
--- /dev/null
+++ b/examples/slam/pose_graph_3d/pose_graph_3d_error_term.h
@@ -0,0 +1,131 @@
+// 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 EXAMPLES_CERES_POSE_GRAPH_3D_ERROR_TERM_H_
+#define EXAMPLES_CERES_POSE_GRAPH_3D_ERROR_TERM_H_
+
+#include "Eigen/Core"
+#include "ceres/autodiff_cost_function.h"
+
+#include "types.h"
+
+namespace ceres {
+namespace examples {
+
+// Computes the error term for two poses that have a relative pose measurement
+// between them. Let the hat variables be the measurement. We have two poses x_a
+// and x_b. Through sensor measurements we can measure the transformation of
+// frame B w.r.t frame A denoted as t_ab_hat. We can compute an error metric
+// between the current estimate of the poses and the measurement.
+//
+// In this formulation, we have chosen to represent the rigid transformation as
+// a Hamiltonian quaternion, q, and position, p. The quaternion ordering is
+// [x, y, z, w].
+
+// The estimated measurement is:
+// t_ab = [ p_ab ] = [ R(q_a)^T * (p_b - p_a) ]
+// [ q_ab ] [ q_a^{-1] * q_b ]
+//
+// where ^{-1} denotes the inverse and R(q) is the rotation matrix for the
+// quaternion. Now we can compute an error metric between the estimated and
+// measurement transformation. For the orientation error, we will use the
+// standard multiplicative error resulting in:
+//
+// error = [ p_ab - \hat{p}_ab ]
+// [ 2.0 * Vec(q_ab * \hat{q}_ab^{-1}) ]
+//
+// where Vec(*) returns the vector (imaginary) part of the quaternion. Since
+// the measurement has an uncertainty associated with how accurate it is, we
+// will weight the errors by the square root of the measurement information
+// matrix:
+//
+// residuals = I^{1/2) * error
+// where I is the information matrix which is the inverse of the covariance.
+class PoseGraph3dErrorTerm {
+ public:
+ PoseGraph3dErrorTerm(const Pose3d& t_ab_measured,
+ const Eigen::Matrix<double, 6, 6>& sqrt_information)
+ : t_ab_measured_(t_ab_measured), sqrt_information_(sqrt_information) {}
+
+ template <typename T>
+ bool operator()(const T* const p_a_ptr, const T* const q_a_ptr,
+ const T* const p_b_ptr, const T* const q_b_ptr,
+ T* residuals_ptr) const {
+ Eigen::Map<const Eigen::Matrix<T, 3, 1> > p_a(p_a_ptr);
+ Eigen::Map<const Eigen::Quaternion<T> > q_a(q_a_ptr);
+
+ Eigen::Map<const Eigen::Matrix<T, 3, 1> > p_b(p_b_ptr);
+ Eigen::Map<const Eigen::Quaternion<T> > q_b(q_b_ptr);
+
+ // Compute the relative transformation between the two frames.
+ Eigen::Quaternion<T> q_a_inverse = q_a.conjugate();
+ Eigen::Quaternion<T> q_ab_estimated = q_a_inverse * q_b;
+
+ // Represent the displacement between the two frames in the A frame.
+ Eigen::Matrix<T, 3, 1> p_ab_estimated = q_a_inverse * (p_b - p_a);
+
+ // Compute the error between the two orientation estimates.
+ Eigen::Quaternion<T> delta_q =
+ t_ab_measured_.q.template cast<T>() * q_ab_estimated.conjugate();
+
+ // Compute the residuals.
+ // [ position ] [ delta_p ]
+ // [ orientation (3x1)] = [ 2 * delta_q(0:2) ]
+ Eigen::Map<Eigen::Matrix<T, 6, 1> > residuals(residuals_ptr);
+ residuals.template block<3, 1>(0, 0) =
+ p_ab_estimated - t_ab_measured_.p.template cast<T>();
+ residuals.template block<3, 1>(3, 0) = T(2.0) * delta_q.vec();
+
+ // Scale the residuals by the measurement uncertainty.
+ residuals.applyOnTheLeft(sqrt_information_.template cast<T>());
+
+ return true;
+ }
+
+ static ceres::CostFunction* Create(
+ const Pose3d& t_ab_measured,
+ const Eigen::Matrix<double, 6, 6>& sqrt_information) {
+ return new ceres::AutoDiffCostFunction<PoseGraph3dErrorTerm, 6, 3, 4, 3, 4>(
+ new PoseGraph3dErrorTerm(t_ab_measured, sqrt_information));
+ }
+
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+ private:
+ // The measurement for the position of B relative to A in the A frame.
+ const Pose3d t_ab_measured_;
+ // The square root of the measurement information matrix.
+ const Eigen::Matrix<double, 6, 6> sqrt_information_;
+};
+
+} // namespace examples
+} // namespace ceres
+
+#endif // EXAMPLES_CERES_POSE_GRAPH_3D_ERROR_TERM_H_
diff --git a/examples/slam/pose_graph_3d/types.h b/examples/slam/pose_graph_3d/types.h
new file mode 100644
index 0000000..2f12501
--- /dev/null
+++ b/examples/slam/pose_graph_3d/types.h
@@ -0,0 +1,114 @@
+// 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 EXAMPLES_CERES_TYPES_H_
+#define EXAMPLES_CERES_TYPES_H_
+
+#include <istream>
+#include <map>
+#include <string>
+#include <vector>
+
+#include "Eigen/Core"
+#include "Eigen/Geometry"
+
+namespace ceres {
+namespace examples {
+
+struct Pose3d {
+ Eigen::Vector3d p;
+ Eigen::Quaterniond q;
+
+ // The name of the data type in the g2o file format.
+ static std::string name() {
+ return "VERTEX_SE3:QUAT";
+ }
+
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+};
+
+std::istream& operator>>(std::istream& input, Pose3d& pose) {
+ input >> pose.p.x() >> pose.p.y() >> pose.p.z() >> pose.q.x() >>
+ pose.q.y() >> pose.q.z() >> pose.q.w();
+ // Normalize the quaternion to account for precision loss due to
+ // serialization.
+ pose.q.normalize();
+ return input;
+}
+
+typedef std::map<int, Pose3d, std::less<int>,
+ Eigen::aligned_allocator<std::pair<const int, Pose3d> > >
+ MapOfPoses;
+
+// The constraint between two vertices in the pose graph. The constraint is the
+// transformation from vertex id_begin to vertex id_end.
+struct Constraint3d {
+ int id_begin;
+ int id_end;
+
+ // The transformation that represents the pose of the end frame E w.r.t. the
+ // begin frame B. In other words, it transforms a vector in the E frame to
+ // the B frame.
+ Pose3d t_be;
+
+ // The inverse of the covariance matrix for the measurement. The order of the
+ // entries are x, y, z, delta orientation.
+ Eigen::Matrix<double, 6, 6> information;
+
+ // The name of the data type in the g2o file format.
+ static std::string name() {
+ return "EDGE_SE3:QUAT";
+ }
+
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+};
+
+std::istream& operator>>(std::istream& input, Constraint3d& constraint) {
+ Pose3d& t_be = constraint.t_be;
+ input >> constraint.id_begin >> constraint.id_end >> t_be;
+
+ for (int i = 0; i < 6 && input.good(); ++i) {
+ for (int j = i; j < 6 && input.good(); ++j) {
+ input >> constraint.information(i, j);
+ if (i != j) {
+ constraint.information(j, i) = constraint.information(i, j);
+ }
+ }
+ }
+ return input;
+}
+
+typedef std::vector<Constraint3d, Eigen::aligned_allocator<Constraint3d> >
+ VectorOfConstraints;
+
+} // namespace examples
+} // namespace ceres
+
+#endif // EXAMPLES_CERES_TYPES_H_