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

Now reads bearing-range factors

release/4.3a0
Frank Dellaert 2012-08-24 22:02:40 +00:00
parent 3dff334cdf
commit 23f3111148
1 changed files with 58 additions and 33 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

91
gtsam/slam/dataset.cpp
View File

@ -24,6 +24,7 @@
#include <gtsam/linear/Sampler.h> #include <gtsam/linear/Sampler.h>
#include <gtsam/slam/dataset.h> #include <gtsam/slam/dataset.h>
#include <gtsam/slam/BetweenFactor.h> #include <gtsam/slam/BetweenFactor.h>
#include <gtsam/slam/BearingRangeFactor.h>
using namespace std; using namespace std;
@ -47,7 +48,7 @@ pair<NonlinearFactorGraph::shared_ptr, Values::shared_ptr> load2D(
if (!is) if (!is)
throw std::invalid_argument("load2D: can not find the file!"); throw std::invalid_argument("load2D: can not find the file!");
Values::shared_ptr poses(new Values); Values::shared_ptr initial(new Values);
NonlinearFactorGraph::shared_ptr graph(new NonlinearFactorGraph); NonlinearFactorGraph::shared_ptr graph(new NonlinearFactorGraph);
string tag; string tag;
@ -63,7 +64,7 @@ pair<NonlinearFactorGraph::shared_ptr, Values::shared_ptr> load2D(
// optional filter // optional filter
if (maxID && id >= maxID) if (maxID && id >= maxID)
continue; continue;
poses->insert(id, Pose2(x, y, yaw)); initial->insert(id, Pose2(x, y, yaw));
} }
is.ignore(LINESIZE, '\n'); is.ignore(LINESIZE, '\n');
} }
@ -77,49 +78,73 @@ pair<NonlinearFactorGraph::shared_ptr, Values::shared_ptr> load2D(
while (is) { while (is) {
is >> tag; is >> tag;
if ((tag == "EDGE2") || (tag == "EDGE") || (tag == "ODOMETRY")) { if ((tag == "EDGE2") || (tag == "EDGE") || (tag == "ODOMETRY")) {
int id1, id2; int id1, id2;
double x, y, yaw; double x, y, yaw;
is >> id1 >> id2 >> x >> y >> yaw; is >> id1 >> id2 >> x >> y >> yaw;
Matrix m = eye(3); Matrix m = eye(3);
is >> m(0, 0) >> m(0, 1) >> m(1, 1) >> m(2, 2) >> m(0, 2) >> m(1, 2); is >> m(0, 0) >> m(0, 1) >> m(1, 1) >> m(2, 2) >> m(0, 2) >> m(1, 2);
m(2, 0) = m(0, 2); m(2, 0) = m(0, 2);
m(2, 1) = m(1, 2); m(2, 1) = m(1, 2);
m(1, 0) = m(0, 1); m(1, 0) = m(0, 1);
// optional filter // optional filter
if (maxID && (id1 >= maxID || id2 >= maxID)) if (maxID && (id1 >= maxID || id2 >= maxID))
continue; continue;
Pose2 l1Xl2(x, y, yaw); Pose2 l1Xl2(x, y, yaw);
// SharedNoiseModel noise = noiseModel::Gaussian::Covariance(m, smart); // SharedNoiseModel noise = noiseModel::Gaussian::Covariance(m, smart);
if (!model) { if (!model) {
Vector variances = Vector_(3, m(0, 0), m(1, 1), m(2, 2)); Vector variances = Vector_(3, m(0, 0), m(1, 1), m(2, 2));
model = noiseModel::Diagonal::Variances(variances, smart); model = noiseModel::Diagonal::Variances(variances, smart);
} }
if (addNoise) if (addNoise)
l1Xl2 = l1Xl2.retract(sampler.sampleNewModel(*model)); l1Xl2 = l1Xl2.retract(sampler.sampleNewModel(*model));
// Insert vertices if pure odometry file // Insert vertices if pure odometry file
if (!poses->exists(id1)) if (!initial->exists(id1))
poses->insert(id1, Pose2()); initial->insert(id1, Pose2());
if (!poses->exists(id2)) if (!initial->exists(id2))
poses->insert(id2, poses->at<Pose2>(id1) * l1Xl2); initial->insert(id2, initial->at<Pose2>(id1) * l1Xl2);
NonlinearFactor::shared_ptr factor( NonlinearFactor::shared_ptr factor(
new BetweenFactor<Pose2>(id1, id2, l1Xl2, *model)); new BetweenFactor<Pose2>(id1, id2, l1Xl2, *model));
graph->push_back(factor); graph->push_back(factor);
} }
if (tag == "BR") {
int id1, id2;
double bearing, range, bearing_std, range_std;
is >> id1 >> id2 >> bearing >> range >> bearing_std >> range_std;
// optional filter
if (maxID && (id1 >= maxID || id2 >= maxID))
continue;
noiseModel::Diagonal::shared_ptr measurementNoise =
noiseModel::Diagonal::Sigmas(Vector_(2, bearing_std, range_std));
graph->add(BearingRangeFactor<Pose2, Point2>(id1, id2, bearing, range, measurementNoise));
// Insert poses or points if they do not exist yet
if (!initial->exists(id1))
initial->insert(id1, Pose2());
if (!initial->exists(id2)) {
Pose2 pose = initial->at<Pose2>(id1);
Point2 local(cos(bearing)*range,sin(bearing)*range);
Point2 global = pose.transform_from(local);
initial->insert(id2, global);
}
}
is.ignore(LINESIZE, '\n'); is.ignore(LINESIZE, '\n');
} }
cout << "load2D read a graph file with " << poses->size() cout << "load2D read a graph file with " << initial->size()
<< " vertices and " << graph->nrFactors() << " factors" << endl; << " vertices and " << graph->nrFactors() << " factors" << endl;
return make_pair(graph, poses); return make_pair(graph, initial);
} }
/* ************************************************************************* */ /* ************************************************************************* */