12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Very generic parseToVector

release/4.3a0
Frank Dellaert 2020-08-12 23:24:52 -04:00
parent a978c15d8e
commit d67afa8a3d
3 changed files with 370 additions and 244 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

576
gtsam/slam/dataset.cpp
View File

File diff suppressed because it is too large Load Diff

24
gtsam/slam/dataset.h
View File

@ -77,16 +77,6 @@ typedef std::pair<Key, Pose2> IndexedPose;
typedef std::pair<Key, Point2> IndexedLandmark;
typedef std::pair<std::pair<Key, Key>, Pose2> IndexedEdge;
#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V41
/**
* Parse TORO/G2O vertex "id x y yaw"
* @param is input stream
* @param tag string parsed from input stream, will only parse if vertex type
*/
GTSAM_EXPORT boost::optional<IndexedPose> parseVertex(std::istream& is,
const std::string& tag);
#endif
/**
* Parse TORO/G2O vertex "id x y yaw"
* @param is input stream
@ -118,7 +108,8 @@ using BetweenFactorPose2s =
/// Parse edges in 2D g2o graph file into a set of BetweenFactors.
GTSAM_EXPORT BetweenFactorPose2s parse2DFactors(
const std::string &filename,
const noiseModel::Diagonal::shared_ptr &corruptingNoise = nullptr);
const noiseModel::Diagonal::shared_ptr &corruptingNoise = nullptr,
Key maxNr = 0);
/// Parse vertices in 2D g2o graph file into a map of Pose2s.
GTSAM_EXPORT std::map<Key, Pose2> parse2DPoses(const std::string &filename,
@ -343,4 +334,15 @@ GTSAM_EXPORT Values initialCamerasEstimate(const SfmData& db);
*/
GTSAM_EXPORT Values initialCamerasAndPointsEstimate(const SfmData& db);
#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V41
/**
* Parse TORO/G2O vertex "id x y yaw"
* @param is input stream
* @param tag string parsed from input stream, will only parse if vertex type
*/
GTSAM_EXPORT boost::optional<IndexedPose> parseVertex(std::istream &is,
const std::string &tag) {
return parseVertexPose(is, tag);
}
#endif
} // namespace gtsam

14
gtsam/slam/tests/testDataset.cpp
View File

@ -104,13 +104,13 @@ TEST(dataSet, load2D) {
boost::dynamic_pointer_cast<BetweenFactor<Pose2>>(graph->at(0));
EXPECT(assert_equal(expected, *actual));
// // Check factor parsing
// const auto actualFactors = parse2DFactors(filename);
// for (size_t i : {0, 1, 2, 3, 4, 5}) {
// EXPECT(assert_equal(
// *boost::dynamic_pointer_cast<BetweenFactor<Pose2>>(graph->at(i)),
// *actualFactors[i], 1e-5));
// }
// Check factor parsing
const auto actualFactors = parse2DFactors(filename);
for (size_t i : {0, 1, 2, 3, 4, 5}) {
EXPECT(assert_equal(
*boost::dynamic_pointer_cast<BetweenFactor<Pose2>>(graph->at(i)),
*actualFactors[i], 1e-5));
}
// Check pose parsing
const auto actualPoses = parse2DPoses(filename);