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

support for landmarks in g2o files

release/4.3a0
Varun Agrawal 2020-07-11 02:52:34 -04:00
parent 5dd1c8e3dc
commit 7d27431572
3 changed files with 128 additions and 18 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

102
gtsam/slam/dataset.cpp
View File

@ -11,7 +11,10 @@
/**
* @file dataset.cpp
* @date Jan 22, 2010
* @author Kai Ni, Luca Carlone, Frank Dellaert
* @author Kai Ni
* @author Luca Carlone
* @author Frank Dellaert
* @author Varun Agrawal
* @brief utility functions for loading datasets
*/
@ -193,7 +196,7 @@ static SharedNoiseModel readNoiseModel(ifstream& is, bool smart,
}
/* ************************************************************************* */
boost::optional<IndexedPose> parseVertex(istream& is, const string& tag) {
boost::optional<IndexedPose> parseVertexPose(istream& is, const string& tag) {
if ((tag == "VERTEX2") || (tag == "VERTEX_SE2") || (tag == "VERTEX")) {
Key id;
double x, y, yaw;
@ -204,6 +207,18 @@ boost::optional<IndexedPose> parseVertex(istream& is, const string& tag) {
}
}
/* ************************************************************************* */
boost::optional<IndexedLandmark> parseVertexLandmark(istream& is, const string& tag) {
if (tag == "VERTEX_XY") {
Key id;
double x, y;
is >> id >> x >> y;
return IndexedLandmark(id, Point2(x, y));
} else {
return boost::none;
}
}
/* ************************************************************************* */
boost::optional<IndexedEdge> parseEdge(istream& is, const string& tag) {
if ((tag == "EDGE2") || (tag == "EDGE") || (tag == "EDGE_SE2")
@ -232,12 +247,12 @@ GraphAndValues load2D(const string& filename, SharedNoiseModel model, Key maxID,
string tag;
// load the poses
// load the poses and landmarks
while (!is.eof()) {
if (!(is >> tag))
break;
const auto indexed_pose = parseVertex(is, tag);
const auto indexed_pose = parseVertexPose(is, tag);
if (indexed_pose) {
Key id = indexed_pose->first;
@ -247,6 +262,16 @@ GraphAndValues load2D(const string& filename, SharedNoiseModel model, Key maxID,
initial->insert(id, indexed_pose->second);
}
const auto indexed_landmark = parseVertexLandmark(is, tag);
if (indexed_landmark) {
Key id = indexed_landmark->first;
// optional filter
if (maxID && id >= maxID)
continue;
initial->insert(id, indexed_landmark->second);
}
is.ignore(LINESIZE, '\n');
}
is.clear(); /* clears the end-of-file and error flags */
@ -429,7 +454,7 @@ void writeG2o(const NonlinearFactorGraph& graph, const Values& estimate,
const string& filename) {
fstream stream(filename.c_str(), fstream::out);
// save 2D & 3D poses
// save 2D poses
for (const auto& key_value : estimate) {
auto p = dynamic_cast<const GenericValue<Pose2>*>(&key_value.value);
if (!p) continue;
@ -438,15 +463,34 @@ void writeG2o(const NonlinearFactorGraph& graph, const Values& estimate,
<< pose.y() << " " << pose.theta() << endl;
}
// save 3D poses
for(const auto& key_value: estimate) {
auto p = dynamic_cast<const GenericValue<Pose3>*>(&key_value.value);
if (!p) continue;
const Pose3& pose = p->value();
const Point3 t = pose.translation();
const auto q = pose.rotation().toQuaternion();
stream << "VERTEX_SE3:QUAT " << key_value.key << " " << t.x() << " "
<< t.y() << " " << t.z() << " " << q.x() << " " << q.y() << " "
<< q.z() << " " << q.w() << endl;
auto p = dynamic_cast<const GenericValue<Pose3>*>(&key_value.value);
if (!p) continue;
const Pose3& pose = p->value();
const Point3 t = pose.translation();
const auto q = pose.rotation().toQuaternion();
stream << "VERTEX_SE3:QUAT " << key_value.key << " " << t.x() << " "
<< t.y() << " " << t.z() << " " << q.x() << " " << q.y() << " "
<< q.z() << " " << q.w() << endl;
}
// save 2D landmarks
for(const auto& key_value: estimate) {
auto p = dynamic_cast<const GenericValue<Point2>*>(&key_value.value);
if (!p) continue;
const Point2& point = p->value();
stream << "VERTEX_XY " << key_value.key << " " << point.x() << " "
<< point.y() << endl;
}
// save 3D landmarks
for(const auto& key_value: estimate) {
auto p = dynamic_cast<const GenericValue<Point3>*>(&key_value.value);
if (!p) continue;
const Point3& point = p->value();
stream << "VERTEX_TRACKXYZ " << key_value.key << " " << point.x() << " "
<< point.y() << " " << point.z() << endl;
}
// save edges (2D or 3D)
@ -515,6 +559,7 @@ static Rot3 NormalizedRot3(double w, double x, double y, double z) {
const double norm = sqrt(w * w + x * x + y * y + z * z), f = 1.0 / norm;
return Rot3::Quaternion(f * w, f * x, f * y, f * z);
}
/* ************************************************************************* */
std::map<Key, Pose3> parse3DPoses(const string& filename) {
ifstream is(filename.c_str());
@ -545,6 +590,30 @@ std::map<Key, Pose3> parse3DPoses(const string& filename) {
return poses;
}
/* ************************************************************************* */
std::map<Key, Point3> parse3DLandmarks(const string& filename) {
ifstream is(filename.c_str());
if (!is)
throw invalid_argument("parse3DLandmarks: can not find file " + filename);
std::map<Key, Point3> landmarks;
while (!is.eof()) {
char buf[LINESIZE];
is.getline(buf, LINESIZE);
istringstream ls(buf);
string tag;
ls >> tag;
if (tag == "VERTEX_TRACKXYZ") {
Key id;
double x, y, z;
ls >> id >> x >> y >> z;
landmarks.emplace(id, Point3(x, y, z));
}
}
return landmarks;
}
/* ************************************************************************* */
BetweenFactorPose3s parse3DFactors(
const string& filename,
@ -617,11 +686,16 @@ GraphAndValues load3D(const string& filename) {
graph->push_back(factor);
}
const auto poses = parse3DPoses(filename);
Values::shared_ptr initial(new Values);
const auto poses = parse3DPoses(filename);
for (const auto& key_pose : poses) {
initial->insert(key_pose.first, key_pose.second);
}
const auto landmarks = parse3DLandmarks(filename);
for (const auto& key_landmark : landmarks) {
initial->insert(key_landmark.first, key_landmark.second);
}
return make_pair(graph, initial);
}

17
gtsam/slam/dataset.h
View File

@ -76,6 +76,7 @@ enum KernelFunctionType {
/// Return type for auxiliary functions
typedef std::pair<Key, Pose2> IndexedPose;
typedef std::pair<Key, Point2> IndexedLandmark;
typedef std::pair<std::pair<Key, Key>, Pose2> IndexedEdge;
/**
@ -83,9 +84,18 @@ typedef std::pair<std::pair<Key, Key>, Pose2> IndexedEdge;
* @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,
GTSAM_EXPORT boost::optional<IndexedPose> parseVertexPose(std::istream& is,
const std::string& tag);
/**
* Parse G2O landmark vertex "id x y"
* @param is input stream
* @param tag string parsed from input stream, will only parse if vertex type
*/
GTSAM_EXPORT boost::optional<IndexedLandmark> parseVertexLandmark(std::istream& is,
const std::string& tag)
/**
* Parse TORO/G2O edge "id1 id2 x y yaw"
* @param is input stream
@ -162,9 +172,12 @@ using BetweenFactorPose3s = std::vector<gtsam::BetweenFactor<Pose3>::shared_ptr>
GTSAM_EXPORT BetweenFactorPose3s parse3DFactors(const std::string& filename,
const noiseModel::Diagonal::shared_ptr& corruptingNoise=nullptr);
/// Parse vertices in 3D TORO graph file into a map of Pose3s.
/// Parse vertices in 3D TORO/g2o graph file into a map of Pose3s.
GTSAM_EXPORT std::map<Key, Pose3> parse3DPoses(const std::string& filename);
/// Parse landmarks in 3D g2o graph file into a map of Point3s.
GTSAM_EXPORT std::map<Key, Point3> parse3DLandmarks(const string& filename)
/// Load TORO 3D Graph
GTSAM_EXPORT GraphAndValues load3D(const std::string& filename);

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

@ -43,13 +43,13 @@ TEST(dataSet, findExampleDataFile) {
}
/* ************************************************************************* */
TEST( dataSet, parseVertex)
TEST( dataSet, parseVertexPose)
{
const string str = "VERTEX2 1 2.000000 3.000000 4.000000";
istringstream is(str);
string tag;
EXPECT(is >> tag);
const auto actual = parseVertex(is, tag);
const auto actual = parseVertexPose(is, tag);
EXPECT(actual);
if (actual) {
EXPECT_LONGS_EQUAL(1, actual->first);
@ -57,6 +57,21 @@ TEST( dataSet, parseVertex)
}
}
/* ************************************************************************* */
TEST( dataSet, parseVertexLandmark)
{
const string str = "VERTEX_XY 1 2.000000 3.000000";
istringstream is(str);
string tag;
EXPECT(is >> tag);
const auto actual = parseVertexLandmark(is, tag);
EXPECT(actual);
if (actual) {
EXPECT_LONGS_EQUAL(1, actual->first);
EXPECT(assert_equal(Point2(2, 3), actual->second));
}
}
/* ************************************************************************* */
TEST( dataSet, parseEdge)
{
@ -182,6 +197,12 @@ TEST(dataSet, readG2o3D) {
EXPECT(assert_equal(poses[j], actualPoses.at(j), 1e-5));
}
// Check landmark parsing
const auto actualLandmarks = parse3DLandmarks(g2oFile);
for (size_t j : {0, 1, 2, 3, 4}) {
EXPECT(assert_equal(poses[j], actualPoses.at(j), 1e-5));
}
// Check graph version
NonlinearFactorGraph::shared_ptr actualGraph;
Values::shared_ptr actualValues;
@ -252,6 +273,8 @@ TEST(dataSet, readG2oCheckDeterminants) {
const Rot3 R = key_value.second.rotation();
EXPECT_DOUBLES_EQUAL(1.0, R.matrix().determinant(), 1e-9);
}
const map<Key, Point3> landmarks = parse3DLandmarks(g2oFile);
EXPECT_LONGS_EQUAL(0, landmarks.size());
}
/* ************************************************************************* */