Split parsing and moved to dataset.*
							parent
							
								
									d8ee79fb8f
								
							
						
					
					
						commit
						a47c52cb5e
					
				|  | @ -409,17 +409,17 @@ void save2D(const NonlinearFactorGraph& graph, const Values& config, | |||
| 
 | ||||
| /* ************************************************************************* */ | ||||
| GraphAndValues readG2o(const string& g2oFile, const bool is3D, | ||||
|     KernelFunctionType kernelFunctionType) { | ||||
|   // just call load2D
 | ||||
|   int maxID = 0; | ||||
|   bool addNoise = false; | ||||
|   bool smart = true; | ||||
| 
 | ||||
|   if(is3D) | ||||
|                        KernelFunctionType kernelFunctionType) { | ||||
|   if (is3D) { | ||||
|     return load3D(g2oFile); | ||||
| 
 | ||||
|   return load2D(g2oFile, SharedNoiseModel(), maxID, addNoise, smart, | ||||
|       NoiseFormatG2O, kernelFunctionType); | ||||
|   } else { | ||||
|     // just call load2D
 | ||||
|     int maxID = 0; | ||||
|     bool addNoise = false; | ||||
|     bool smart = true; | ||||
|     return load2D(g2oFile, SharedNoiseModel(), maxID, addNoise, smart, | ||||
|                   NoiseFormatG2O, kernelFunctionType); | ||||
|   } | ||||
| } | ||||
| 
 | ||||
| /* ************************************************************************* */ | ||||
|  | @ -510,15 +510,12 @@ void writeG2o(const NonlinearFactorGraph& graph, const Values& estimate, | |||
| } | ||||
| 
 | ||||
| /* ************************************************************************* */ | ||||
| GraphAndValues load3D(const string& filename) { | ||||
| 
 | ||||
| std::map<Key, Pose3> parse3DPoses(const string& filename) { | ||||
|   ifstream is(filename.c_str()); | ||||
|   if (!is) | ||||
|     throw invalid_argument("load3D: can not find file " + filename); | ||||
| 
 | ||||
|   Values::shared_ptr initial(new Values); | ||||
|   NonlinearFactorGraph::shared_ptr graph(new NonlinearFactorGraph); | ||||
|     throw invalid_argument("parse3DPoses: can not find file " + filename); | ||||
| 
 | ||||
|   std::map<Key, Pose3> poses; | ||||
|   while (!is.eof()) { | ||||
|     char buf[LINESIZE]; | ||||
|     is.getline(buf, LINESIZE); | ||||
|  | @ -530,22 +527,24 @@ GraphAndValues load3D(const string& filename) { | |||
|       Key id; | ||||
|       double x, y, z, roll, pitch, yaw; | ||||
|       ls >> id >> x >> y >> z >> roll >> pitch >> yaw; | ||||
|       Rot3 R = Rot3::Ypr(yaw,pitch,roll); | ||||
|       Point3 t = Point3(x, y, z); | ||||
|       initial->insert(id, Pose3(R,t)); | ||||
|       poses.emplace(id, Pose3(Rot3::Ypr(yaw, pitch, roll), {x, y, z})); | ||||
|     } | ||||
|     if (tag == "VERTEX_SE3:QUAT") { | ||||
|       Key id; | ||||
|       double x, y, z, qx, qy, qz, qw; | ||||
|       ls >> id >> x >> y >> z >> qx >> qy >> qz >> qw; | ||||
|       Rot3 R = Rot3::Quaternion(qw, qx, qy, qz); | ||||
|       Point3 t = Point3(x, y, z); | ||||
|       initial->insert(id, Pose3(R,t)); | ||||
|       poses.emplace(id, Pose3(Rot3::Quaternion(qw, qx, qy, qz), {x, y, z})); | ||||
|     } | ||||
|   } | ||||
|   is.clear(); /* clears the end-of-file and error flags */ | ||||
|   is.seekg(0, ios::beg); | ||||
|   return poses; | ||||
| } | ||||
| 
 | ||||
| /* ************************************************************************* */ | ||||
| std::vector<BetweenFactor<Pose3>::shared_ptr> parse3DFactors(const string& filename) { | ||||
|   ifstream is(filename.c_str()); | ||||
|   if (!is) throw invalid_argument("parse3DFactors: can not find file " + filename); | ||||
| 
 | ||||
|   std::vector<BetweenFactor<Pose3>::shared_ptr> factors; | ||||
|   while (!is.eof()) { | ||||
|     char buf[LINESIZE]; | ||||
|     is.getline(buf, LINESIZE); | ||||
|  | @ -557,44 +556,55 @@ GraphAndValues load3D(const string& filename) { | |||
|       Key id1, id2; | ||||
|       double x, y, z, roll, pitch, yaw; | ||||
|       ls >> id1 >> id2 >> x >> y >> z >> roll >> pitch >> yaw; | ||||
|       Rot3 R = Rot3::Ypr(yaw,pitch,roll); | ||||
|       Point3 t = Point3(x, y, z); | ||||
|       Matrix m = I_6x6; | ||||
|       for (int i = 0; i < 6; i++) | ||||
|         for (int j = i; j < 6; j++) | ||||
|           ls >> m(i, j); | ||||
|       Matrix m(6, 6); | ||||
|       for (size_t i = 0; i < 6; i++) | ||||
|         for (size_t j = i; j < 6; j++) ls >> m(i, j); | ||||
|       SharedNoiseModel model = noiseModel::Gaussian::Information(m); | ||||
|       NonlinearFactor::shared_ptr factor( | ||||
|           new BetweenFactor<Pose3>(id1, id2, Pose3(R,t), model)); | ||||
|       graph->push_back(factor); | ||||
|       factors.emplace_back(new BetweenFactor<Pose3>( | ||||
|           id1, id2, Pose3(Rot3::Ypr(yaw, pitch, roll), {x, y, z}), model)); | ||||
|     } | ||||
|     if (tag == "EDGE_SE3:QUAT") { | ||||
|       Matrix m = I_6x6; | ||||
|       Key id1, id2; | ||||
|       double x, y, z, qx, qy, qz, qw; | ||||
|       ls >> id1 >> id2 >> x >> y >> z >> qx >> qy >> qz >> qw; | ||||
|       Rot3 R = Rot3::Quaternion(qw, qx, qy, qz); | ||||
|       Point3 t = Point3(x, y, z); | ||||
|       for (int i = 0; i < 6; i++){ | ||||
|         for (int j = i; j < 6; j++){ | ||||
|       Matrix m(6, 6); | ||||
|       for (size_t i = 0; i < 6; i++) { | ||||
|         for (size_t j = i; j < 6; j++) { | ||||
|           double mij; | ||||
|           ls >> mij; | ||||
|           m(i, j) = mij; | ||||
|           m(j, i) = mij; | ||||
|         } | ||||
|       } | ||||
|       Matrix mgtsam = I_6x6; | ||||
|       Matrix mgtsam(6, 6); | ||||
| 
 | ||||
|       mgtsam.block<3,3>(0,0) = m.block<3,3>(3,3); // cov rotation
 | ||||
|       mgtsam.block<3,3>(3,3) = m.block<3,3>(0,0); // cov translation
 | ||||
|       mgtsam.block<3,3>(0,3) = m.block<3,3>(0,3); // off diagonal
 | ||||
|       mgtsam.block<3,3>(3,0) = m.block<3,3>(3,0); // off diagonal
 | ||||
|       mgtsam.block<3, 3>(0, 0) = m.block<3, 3>(3, 3);  // cov rotation
 | ||||
|       mgtsam.block<3, 3>(3, 3) = m.block<3, 3>(0, 0);  // cov translation
 | ||||
|       mgtsam.block<3, 3>(0, 3) = m.block<3, 3>(0, 3);  // off diagonal
 | ||||
|       mgtsam.block<3, 3>(3, 0) = m.block<3, 3>(3, 0);  // off diagonal
 | ||||
| 
 | ||||
|       SharedNoiseModel model = noiseModel::Gaussian::Information(mgtsam); | ||||
|       NonlinearFactor::shared_ptr factor(new BetweenFactor<Pose3>(id1, id2, Pose3(R,t), model)); | ||||
|       graph->push_back(factor); | ||||
|       factors.emplace_back(new BetweenFactor<Pose3>( | ||||
|           id1, id2, Pose3(Rot3::Quaternion(qw, qx, qy, qz), {x, y, z}), model)); | ||||
|     } | ||||
|   } | ||||
|   return factors; | ||||
| } | ||||
| 
 | ||||
| /* ************************************************************************* */ | ||||
| GraphAndValues load3D(const string& filename) { | ||||
|   const auto factors = parse3DFactors(filename); | ||||
|   NonlinearFactorGraph::shared_ptr graph(new NonlinearFactorGraph); | ||||
|   for (const auto& factor : factors) { | ||||
|     graph->push_back(factor); | ||||
|   } | ||||
| 
 | ||||
|   const auto poses = parse3DPoses(filename); | ||||
|   Values::shared_ptr initial(new Values); | ||||
|   for (const auto& key_pose : poses) { | ||||
|     initial->insert(key_pose.first, key_pose.second); | ||||
|   } | ||||
| 
 | ||||
|   return make_pair(graph, initial); | ||||
| } | ||||
| 
 | ||||
|  |  | |||
|  | @ -153,9 +153,13 @@ GTSAM_EXPORT GraphAndValues readG2o(const std::string& g2oFile, const bool is3D | |||
| GTSAM_EXPORT void writeG2o(const NonlinearFactorGraph& graph, | ||||
|     const Values& estimate, const std::string& filename); | ||||
| 
 | ||||
| /**
 | ||||
|  * Load TORO 3D Graph | ||||
|  */ | ||||
| /// Parse edges in 3D TORO graph file into a set of BetweenFactors.
 | ||||
| GTSAM_EXPORT std::vector<BetweenFactor<Pose3>::shared_ptr> parse3DFactors(const std::string& filename); | ||||
| 
 | ||||
| /// Parse vertices in 3D TORO graph file into a map of Pose3s.
 | ||||
| GTSAM_EXPORT std::map<Key, Pose3> parse3DPoses(const std::string& filename); | ||||
| 
 | ||||
| /// Load TORO 3D Graph
 | ||||
| GTSAM_EXPORT GraphAndValues load3D(const std::string& filename); | ||||
| 
 | ||||
| /// A measurement with its camera index
 | ||||
|  |  | |||
|  | @ -162,90 +162,6 @@ TEST( dataSet, readG2o) | |||
| } | ||||
| 
 | ||||
| /* ************************************************************************* */ | ||||
| using BetweenFactors = std::vector<BetweenFactor<Pose3>::shared_ptr>; | ||||
| using Poses = std::map<Key, Pose3>; | ||||
| std::pair<BetweenFactors, Poses> parseG2o3D(const std::string&); | ||||
| 
 | ||||
| #include <fstream> | ||||
| #define LINESIZE 81920 | ||||
| 
 | ||||
| /* ************************************************************************* */ | ||||
| std::pair<BetweenFactors, Poses> parseG2o3D(const string& filename) { | ||||
|   ifstream is(filename.c_str()); | ||||
|   if (!is) throw invalid_argument("load3D: can not find file " + filename); | ||||
| 
 | ||||
|   BetweenFactors factors; | ||||
|   Poses poses; | ||||
| 
 | ||||
|   while (!is.eof()) { | ||||
|     char buf[LINESIZE]; | ||||
|     is.getline(buf, LINESIZE); | ||||
|     istringstream ls(buf); | ||||
|     string tag; | ||||
|     ls >> tag; | ||||
| 
 | ||||
|     if (tag == "VERTEX3") { | ||||
|       Key id; | ||||
|       double x, y, z, roll, pitch, yaw; | ||||
|       ls >> id >> x >> y >> z >> roll >> pitch >> yaw; | ||||
|       poses.emplace(id, Pose3(Rot3::Ypr(yaw, pitch, roll), {x, y, z})); | ||||
|     } | ||||
|     if (tag == "VERTEX_SE3:QUAT") { | ||||
|       Key id; | ||||
|       double x, y, z, qx, qy, qz, qw; | ||||
|       ls >> id >> x >> y >> z >> qx >> qy >> qz >> qw; | ||||
|       poses.emplace(id, Pose3(Rot3::Quaternion(qw, qx, qy, qz), {x, y, z})); | ||||
|     } | ||||
|   } | ||||
|   is.clear(); /* clears the end-of-file and error flags */ | ||||
|   is.seekg(0, ios::beg); | ||||
| 
 | ||||
|   while (!is.eof()) { | ||||
|     char buf[LINESIZE]; | ||||
|     is.getline(buf, LINESIZE); | ||||
|     istringstream ls(buf); | ||||
|     string tag; | ||||
|     ls >> tag; | ||||
| 
 | ||||
|     if (tag == "EDGE3") { | ||||
|       Key id1, id2; | ||||
|       double x, y, z, roll, pitch, yaw; | ||||
|       ls >> id1 >> id2 >> x >> y >> z >> roll >> pitch >> yaw; | ||||
|       Matrix m(6, 6); | ||||
|       for (size_t i = 0; i < 6; i++) | ||||
|         for (size_t j = i; j < 6; j++) ls >> m(i, j); | ||||
|       SharedNoiseModel model = noiseModel::Gaussian::Information(m); | ||||
|       factors.emplace_back(new BetweenFactor<Pose3>( | ||||
|           id1, id2, Pose3(Rot3::Ypr(yaw, pitch, roll), {x, y, z}), model)); | ||||
|     } | ||||
|     if (tag == "EDGE_SE3:QUAT") { | ||||
|       Key id1, id2; | ||||
|       double x, y, z, qx, qy, qz, qw; | ||||
|       ls >> id1 >> id2 >> x >> y >> z >> qx >> qy >> qz >> qw; | ||||
|       Matrix m(6, 6); | ||||
|       for (size_t i = 0; i < 6; i++) { | ||||
|         for (size_t j = i; j < 6; j++) { | ||||
|           double mij; | ||||
|           ls >> mij; | ||||
|           m(i, j) = mij; | ||||
|           m(j, i) = mij; | ||||
|         } | ||||
|       } | ||||
|       Matrix mgtsam(6, 6); | ||||
| 
 | ||||
|       mgtsam.block<3, 3>(0, 0) = m.block<3, 3>(3, 3);  // cov rotation
 | ||||
|       mgtsam.block<3, 3>(3, 3) = m.block<3, 3>(0, 0);  // cov translation
 | ||||
|       mgtsam.block<3, 3>(0, 3) = m.block<3, 3>(0, 3);  // off diagonal
 | ||||
|       mgtsam.block<3, 3>(3, 0) = m.block<3, 3>(3, 0);  // off diagonal
 | ||||
| 
 | ||||
|       SharedNoiseModel model = noiseModel::Gaussian::Information(mgtsam); | ||||
|       factors.emplace_back(new BetweenFactor<Pose3>( | ||||
|           id1, id2, Pose3(Rot3::Quaternion(qw, qx, qy, qz), {x, y, z}), model)); | ||||
|     } | ||||
|   } | ||||
|   return make_pair(factors, poses); | ||||
| } | ||||
| 
 | ||||
| TEST(dataSet, readG2o3D) { | ||||
|   const string g2oFile = findExampleDataFile("pose3example"); | ||||
|   auto model = noiseModel::Isotropic::Precision(6, 10000); | ||||
|  | @ -291,33 +207,26 @@ TEST(dataSet, readG2o3D) { | |||
|         keys.first, keys.second, relative_poses[i++], model); | ||||
|   } | ||||
| 
 | ||||
|   // Call parse version
 | ||||
|   BetweenFactors actualFactors; | ||||
|   Poses actualPoses; | ||||
|   std::tie(actualFactors, actualPoses) = parseG2o3D(g2oFile); | ||||
| 
 | ||||
|   // Check factors
 | ||||
|   // Check factor parsing
 | ||||
|   const auto actualFactors = parse3DFactors(g2oFile); | ||||
|   for (size_t i : {0, 1, 2, 3, 4, 5}) { | ||||
|     EXPECT(assert_equal( | ||||
|         *boost::dynamic_pointer_cast<BetweenFactor<Pose3>>(expectedGraph[i]), | ||||
|         *actualFactors[i], 1e-5)); | ||||
|   } | ||||
| 
 | ||||
|   // Check poses
 | ||||
|   // Check pose parsing
 | ||||
|   const auto actualPoses = parse3DPoses(g2oFile); | ||||
|   for (size_t j : {0, 1, 2, 3, 4}) { | ||||
|     EXPECT(assert_equal(poses[j], actualPoses.at(j), 1e-5)); | ||||
|   } | ||||
| 
 | ||||
|   // Call graph version
 | ||||
|   // Check graph version
 | ||||
|   NonlinearFactorGraph::shared_ptr actualGraph; | ||||
|   Values::shared_ptr actualValues; | ||||
|   bool is3D = true; | ||||
|   boost::tie(actualGraph, actualValues) = readG2o(g2oFile, is3D); | ||||
| 
 | ||||
|   // Check factor graph
 | ||||
|   EXPECT(assert_equal(expectedGraph, *actualGraph, 1e-5)); | ||||
| 
 | ||||
|   // Check values
 | ||||
|   for (size_t j : {0, 1, 2, 3, 4}) { | ||||
|     EXPECT(assert_equal(poses[j], actualValues->at<Pose3>(j), 1e-5)); | ||||
|   } | ||||
|  |  | |||
		Loading…
	
		Reference in New Issue