Add an example for modeling and solving a 3D pose graph SLAM problem. Change-Id: I750ca5f20c495edfee5f60ffedccc5bd8ba2bb37
diff --git a/docs/source/nnls_tutorial.rst b/docs/source/nnls_tutorial.rst index 3a2cc65..8882cad 100644 --- a/docs/source/nnls_tutorial.rst +++ b/docs/source/nnls_tutorial.rst
@@ -913,36 +913,125 @@ 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 + .. code-block:: bash - /path/to/bin/pose_graph_2d /path/to/dataset/dataset.g2o + /path/to/bin/pose_graph_2d /path/to/dataset/dataset.g2o - A python script is provided to visualize the resulting output files. + A python script is provided to visualize the resulting output files. - .. code-block:: bash + .. code-block:: bash - /path/to/repo/examples/slam/pose_graph_2d/plot_results.py --optimized_poses ./poses_optimized.txt --initial_poses ./poses_original.txt + /path/to/repo/examples/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: + 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 + .. 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. + 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 + .. 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. + .. [#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. + .. [#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. + +#. `slam/pose_graph_3d/pose_graph_3d.cc + <https://ceres-solver.googlesource.com/ceres-solver/+/master/examples/slam/pose_graph_3d/pose_graph_3d.cc>`_ + The following explains how to formulate the pose graph based SLAM problem in + 3-Dimensions with relative pose constraints. The example also illustrates how + to use Eigen's geometry module with Ceres's automatic differentiation + functionality. + + The robot at timestamp :math:`t` has state :math:`x_t = [p^T, q^T]^T` where + :math:`p` is a 3D vector that represents the position and :math:`q` is the + orientation represented as an Eigen quaternion. 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{q}_{ab}^T]^T`. + 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(q_a)^{T} (p_b - p_a) - \hat{p}_{ab} \\ + 2.0 \mathrm{vec}\left((q_a^{-1} q_b) \hat{q}_{ab}^{-1}\right) + \end{array} + \right] + + where the function :math:`\mathrm{vec}()` returns the vector part of the + quaternion, i.e. :math:`[q_x, q_y, q_z]`, and :math:`R(q)` is the rotation + matrix for the quaternion. + + 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. + + Given that we are using a quaternion to represent the orientation, we need to + use a local parameterization (:class:`EigenQuaternionParameterization`) to + only apply updates orthogonal to the 4-vector defining the + quaternion. Eigen's quaternion uses a different internal memory layout for + the elements of the quaternion than what is commonly used. Specifically, + Eigen stores the elements in memory as :math:`[x, y, z, w]` where the real + part is last whereas it is typically stored first. Note, when creating an + Eigen quaternion through the constructor the elements are accepted in + :math:`w`, :math:`x`, :math:`y`, :math:`z` order. Since Ceres operates on + parameter blocks which are raw double pointers this difference is important + and requires a different parameterization. + + This package includes an executable :member:`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. :member:`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: + + .. code-block:: bash + + 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 :member:`pose_graph_3d` expects the first argument to be the + path to the problem definition. The executable can be run via + + .. code-block:: bash + + /path/to/bin/pose_graph_3d /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`` + + .. code-block:: bash + + /path/to/repo/examples/slam/pose_graph_3d/plot_results.py --optimized_poses ./poses_optimized.txt --initial_poses ./poses_original.txt + + As an example, a standard synthetic benchmark dataset [#f9]_ where the robot is + traveling on the surface of a sphere which has 2500 nodes with a total of + 4949 edges was solved. Visualizing the results with the provided script + produces: + + .. figure:: pose_graph_3d_ex.png + :figwidth: 600px + :height: 300px + :align: center
diff --git a/docs/source/pose_graph_3d_ex.png b/docs/source/pose_graph_3d_ex.png new file mode 100644 index 0000000..ae2cfc3 --- /dev/null +++ b/docs/source/pose_graph_3d_ex.png Binary files differ
diff --git a/examples/slam/CMakeLists.txt b/examples/slam/CMakeLists.txt index d2c0d42..a5f12c4 100644 --- a/examples/slam/CMakeLists.txt +++ b/examples/slam/CMakeLists.txt
@@ -27,4 +27,10 @@ # POSSIBILITY OF SUCH DAMAGE. # # Author: vitus@google.com (Michael Vitus) +include_directories(./) + add_subdirectory(pose_graph_2d) + +if (GFLAGS) + add_subdirectory(pose_graph_3d) +endif (GFLAGS) \ No newline at end of file
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 index 117f4cd..5574ad1 100644 --- a/examples/slam/pose_graph_2d/CMakeLists.txt +++ b/examples/slam/pose_graph_2d/CMakeLists.txt
@@ -33,7 +33,6 @@ normalize_angle.h pose_graph_2d.cc pose_graph_2d_error_term.h - read_g2o.cc - ) + types.h) target_link_libraries(pose_graph_2d ceres)
diff --git a/examples/slam/pose_graph_2d/pose_graph_2d.cc b/examples/slam/pose_graph_2d/pose_graph_2d.cc index e6caf7e..969bdb5 100644 --- a/examples/slam/pose_graph_2d/pose_graph_2d.cc +++ b/examples/slam/pose_graph_2d/pose_graph_2d.cc
@@ -41,8 +41,8 @@ #include "angle_local_parameterization.h" #include "ceres/ceres.h" +#include "common/read_g2o.h" #include "pose_graph_2d_error_term.h" -#include "read_g2o.h" #include "types.h" namespace ceres {
diff --git a/examples/slam/pose_graph_2d/read_g2o.cc b/examples/slam/pose_graph_2d/read_g2o.cc deleted file mode 100644 index 22f424b..0000000 --- a/examples/slam/pose_graph_2d/read_g2o.cc +++ /dev/null
@@ -1,122 +0,0 @@ -// 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 deleted file mode 100644 index dfa30d6..0000000 --- a/examples/slam/pose_graph_2d/read_g2o.h +++ /dev/null
@@ -1,64 +0,0 @@ -// 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 index d54c733..a54d9bf 100644 --- a/examples/slam/pose_graph_2d/types.h +++ b/examples/slam/pose_graph_2d/types.h
@@ -35,7 +35,10 @@ #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 { @@ -45,8 +48,20 @@ 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 { @@ -60,8 +75,30 @@ // 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
diff --git a/examples/slam/pose_graph_3d/CMakeLists.txt b/examples/slam/pose_graph_3d/CMakeLists.txt new file mode 100644 index 0000000..1be4e85 --- /dev/null +++ b/examples/slam/pose_graph_3d/CMakeLists.txt
@@ -0,0 +1,32 @@ +# 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_3d pose_graph_3d.cc) +target_link_libraries(pose_graph_3d ceres ${GFLAGS_LIBRARIES})
diff --git a/examples/slam/pose_graph_3d/README.md b/examples/slam/pose_graph_3d/README.md new file mode 100644 index 0000000..a16aa09 --- /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` expects the first argument to be the path to the +problem definition. To run the executable, +``` +/path/to/bin/pose_graph_3d /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..c7c47fd --- /dev/null +++ b/examples/slam/pose_graph_3d/pose_graph_3d.cc
@@ -0,0 +1,175 @@ +// 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 "glog/logging.h" +#include "pose_graph_3d_error_term.h" +#include "types.h" + +DEFINE_string(input_filename, "", + "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_filename != "") << "Need to specify the filename to read."; + + ceres::examples::MapOfPoses poses; + ceres::examples::VectorOfConstraints constraints; + + CHECK( + ceres::examples::ReadG2oFile(FLAGS_input_filename, &poses, &constraints)) + << "Error reading the file: " << FLAGS_input_filename; + + 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_
