123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141 |
- #ifndef EXAMPLES_CERES_READ_G2O_H_
- #define EXAMPLES_CERES_READ_G2O_H_
- #include <fstream>
- #include <string>
- #include "glog/logging.h"
- namespace ceres::examples {
- 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;
-
- if (poses->find(id) != poses->end()) {
- LOG(ERROR) << "Duplicate vertex with ID: " << id;
- return false;
- }
- (*poses)[id] = pose;
- return true;
- }
- template <typename Constraint, typename Allocator>
- void ReadConstraint(std::ifstream* infile,
- std::vector<Constraint, Allocator>* constraints) {
- Constraint constraint;
- *infile >> constraint;
- constraints->push_back(constraint);
- }
- 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 != nullptr);
- CHECK(constraints != nullptr);
- poses->clear();
- constraints->clear();
- std::ifstream infile(filename.c_str());
- if (!infile) {
- return false;
- }
- std::string data_type;
- while (infile.good()) {
-
- 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;
- }
-
- infile >> std::ws;
- }
- return true;
- }
- }
- #endif
|