examples/slam/common/read_g2o.h

   1 // Ceres Solver - A fast non-linear least squares minimizer
   2 // Copyright 2016 Google Inc. All rights reserved.
   3 // http://ceres-solver.org/
   4 //
   5 // Redistribution and use in source and binary forms, with or without
   6 // modification, are permitted provided that the following conditions are met:
   7 //
   8 // * Redistributions of source code must retain the above copyright notice,
   9 //   this list of conditions and the following disclaimer.
  10 // * Redistributions in binary form must reproduce the above copyright notice,
  11 //   this list of conditions and the following disclaimer in the documentation
  12 //   and/or other materials provided with the distribution.
  13 // * Neither the name of Google Inc. nor the names of its contributors may be
  14 //   used to endorse or promote products derived from this software without
  15 //   specific prior written permission.
  16 //
  17 // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
  18 // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
  19 // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
  20 // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
  21 // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
  22 // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
  23 // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
  24 // INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
  25 // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
  26 // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
  27 // POSSIBILITY OF SUCH DAMAGE.
  28 //
  29 // Author: vitus@google.com (Michael Vitus)
  30 //
  31 // Reads a file in the g2o filename format that describes a pose graph problem.
  32
  33 #ifndef EXAMPLES_CERES_READ_G2O_H_
  34 #define EXAMPLES_CERES_READ_G2O_H_
  35
  36 #include <fstream>
  37 #include <string>
  38
  39 #include "glog/logging.h"
  40
  41 namespace ceres {
  42 namespace examples {
  43
  44 // Reads a single pose from the input and inserts it into the map. Returns false
  45 // if there is a duplicate entry.
  46 template <typename Pose, typename Allocator>
  47 bool ReadVertex(std::ifstream* infile,
  48                 std::map<int, Pose, std::less<int>, Allocator>* poses) {
  49   int id;
  50   Pose pose;
  51   *infile >> id >> pose;
  52
  53   // Ensure we don't have duplicate poses.
  54   if (poses->find(id) != poses->end()) {
  55     LOG(ERROR) << "Duplicate vertex with ID: " << id;
  56     return false;
  57   }
  58   (*poses)[id] = pose;
  59
  60   return true;
  61 }
  62
  63 // Reads the contraints between two vertices in the pose graph
  64 template <typename Constraint, typename Allocator>
  65 void ReadConstraint(std::ifstream* infile,
  66                     std::vector<Constraint, Allocator>* constraints) {
  67   Constraint constraint;
  68   *infile >> constraint;
  69
  70   constraints->push_back(constraint);
  71 }
  72
  73 // Reads a file in the g2o filename format that describes a pose graph
  74 // problem. The g2o format consists of two entries, vertices and constraints.
  75 //
  76 // In 2D, a vertex is defined as follows:
  77 //
  78 // VERTEX_SE2 ID x_meters y_meters yaw_radians
  79 //
  80 // A constraint is defined as follows:
  81 //
  82 // 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
  83 //
  84 // where I_ij is the (i, j)-th entry of the information matrix for the
  85 // measurement.
  86 //
  87 //
  88 // In 3D, a vertex is defined as follows:
  89 //
  90 // VERTEX_SE3:QUAT ID x y z q_x q_y q_z q_w
  91 //
  92 // where the quaternion is in Hamilton form.
  93 // A constraint is defined as follows:
  94 //
  95 // 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
  96 //
  97 // where I_ij is the (i, j)-th entry of the information matrix for the
  98 // measurement. Only the upper-triangular part is stored. The measurement order
  99 // is the delta position followed by the delta orientation.
 100 template <typename Pose, typename Constraint, typename MapAllocator,
 101           typename VectorAllocator>
 102 bool ReadG2oFile(const std::string& filename,
 103                  std::map<int, Pose, std::less<int>, MapAllocator>* poses,
 104                  std::vector<Constraint, VectorAllocator>* constraints) {
 105   CHECK(poses != NULL);
 106   CHECK(constraints != NULL);
 107
 108   poses->clear();
 109   constraints->clear();
 110
 111   std::ifstream infile(filename.c_str());
 112   if (!infile) {
 113     return false;
 114   }
 115
 116   std::string data_type;
 117   while (infile.good()) {
 118     // Read whether the type is a node or a constraint.
 119     infile >> data_type;
 120     if (data_type == Pose::name()) {
 121       if (!ReadVertex(&infile, poses)) {
 122         return false;
 123       }
 124     } else if (data_type == Constraint::name()) {
 125       ReadConstraint(&infile, constraints);
 126     } else {
 127       LOG(ERROR) << "Unknown data type: " << data_type;
 128       return false;
 129     }
 130
 131     // Clear any trailing whitespace from the line.
 132     infile >> std::ws;
 133   }
 134
 135   return true;
 136 }
 137
 138 }  // namespace examples
 139 }  // namespace ceres
 140
 141 #endif  // EXAMPLES_CERES_READ_G2O_H_