examples/slam/pose_graph_3d/README.md

   1 Pose Graph 3D
   2 ----------------
   3
   4 The Simultaneous Localization and Mapping (SLAM) problem consists of building a
   5 map of an unknown environment while simultaneously localizing against this
   6 map. The main difficulty of this problem stems from not having any additional
   7 external aiding information such as GPS. SLAM has been considered one of the
   8 fundamental challenges of robotics. A pose graph optimization problem is one
   9 example of a SLAM problem.
  10
  11 The example also illustrates how to use Eigen's geometry module with Ceres'
  12 automatic differentiation functionality. To represent the orientation, we will
  13 use Eigen's quaternion which uses the Hamiltonian convention but has different
  14 element ordering as compared with Ceres's rotation representation. Specifically
  15 they differ by whether the scalar component q_w is first or last; the element
  16 order for Ceres's quaternion is [q_w, q_x, q_y, q_z] where as Eigen's quaternion
  17 is [q_x, q_y, q_z, q_w].
  18
  19 This package defines the necessary Ceres cost functions needed to model the
  20 3-dimensional pose graph optimization problem as well as a binary to build and
  21 solve the problem. The cost functions are shown for instruction purposes and can
  22 be speed up by using analytical derivatives which take longer to implement.
  23
  24
  25 Running
  26 -----------
  27 This package includes an executable `pose_graph_3d` that will read a problem
  28 definition file. This executable can work with any 3D problem definition that
  29 uses the g2o format with quaternions used for the orientation representation. It
  30 would be relatively straightforward to implement a new reader for a different
  31 format such as TORO or others. `pose_graph_3d` will print the Ceres solver full
  32 summary and then output to disk the original and optimized poses
  33 (`poses_original.txt` and `poses_optimized.txt`, respectively) of the robot in
  34 the following format:
  35 ```
  36 pose_id x y z q_x q_y q_z q_w
  37 pose_id x y z q_x q_y q_z q_w
  38 pose_id x y z q_x q_y q_z q_w
  39 ...
  40 ```
  41 where `pose_id` is the corresponding integer ID from the file definition. Note,
  42 the file will be sorted in ascending order for the ```pose_id```.
  43
  44 The executable `pose_graph_3d` expects the first argument to be the path to the
  45 problem definition. To run the executable,
  46 ```
  47 /path/to/bin/pose_graph_3d /path/to/dataset/dataset.g2o
  48 ```
  49
  50 A script is provided to visualize the resulting output files. There is also an
  51 option to enable equal axes using ```--axes_equal```.
  52 ```
  53 /path/to/repo/examples/slam/pose_graph_3d/plot_results.py --optimized_poses ./poses_optimized.txt --initial_poses ./poses_original.txt
  54 ```