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

41
gtsam/slam/dataset.cpp
View File

@ -24,6 +24,7 @@
#include <gtsam/linear/Sampler.h>
#include <gtsam/slam/dataset.h>
#include <gtsam/slam/BetweenFactor.h>
#include <gtsam/slam/BearingRangeFactor.h>
using namespace std;
@ -47,7 +48,7 @@ pair<NonlinearFactorGraph::shared_ptr, Values::shared_ptr> load2D(
if (!is)
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);
string tag;
@ -63,7 +64,7 @@ pair<NonlinearFactorGraph::shared_ptr, Values::shared_ptr> load2D(
// optional filter
if (maxID && id >= maxID)
continue;
poses->insert(id, Pose2(x, y, yaw));
initial->insert(id, Pose2(x, y, yaw));
}
is.ignore(LINESIZE, '\n');
}
@ -104,22 +105,46 @@ pair<NonlinearFactorGraph::shared_ptr, Values::shared_ptr> load2D(
l1Xl2 = l1Xl2.retract(sampler.sampleNewModel(*model));
// Insert vertices if pure odometry file
if (!poses->exists(id1))
poses->insert(id1, Pose2());
if (!poses->exists(id2))
poses->insert(id2, poses->at<Pose2>(id1) * l1Xl2);
if (!initial->exists(id1))
initial->insert(id1, Pose2());
if (!initial->exists(id2))
initial->insert(id2, initial->at<Pose2>(id1) * l1Xl2);
NonlinearFactor::shared_ptr factor(
new BetweenFactor<Pose2>(id1, id2, l1Xl2, *model));
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');
}
cout << "load2D read a graph file with " << poses->size()
cout << "load2D read a graph file with " << initial->size()
<< " vertices and " << graph->nrFactors() << " factors" << endl;
return make_pair(graph, poses);
return make_pair(graph, initial);
}
/* ************************************************************************* */