Fixed a problem with initialization (exposed by victoria_park.txt) and cleaned up a bit. Also fixed variances -> std deviation.
parent
020d2e43f8
commit
f4cad73b73
|
@ -112,14 +112,14 @@ pair<NonlinearFactorGraph::shared_ptr, Values::shared_ptr> load2D(
|
|||
// Create a sampler with random number generator
|
||||
Sampler sampler(42u);
|
||||
|
||||
// load the factors
|
||||
// Parse the pose constraints
|
||||
int id1, id2;
|
||||
bool haveLandmark = false;
|
||||
while (is) {
|
||||
if(! (is >> tag))
|
||||
break;
|
||||
|
||||
if ((tag == "EDGE2") || (tag == "EDGE") || (tag == "ODOMETRY")) {
|
||||
int id1, id2;
|
||||
double x, y, yaw;
|
||||
double v1, v2, v3, v4, v5, v6;
|
||||
|
||||
|
@ -168,68 +168,75 @@ pair<NonlinearFactorGraph::shared_ptr, Values::shared_ptr> load2D(
|
|||
new BetweenFactor<Pose2>(id1, id2, l1Xl2, *model));
|
||||
graph->push_back(factor);
|
||||
}
|
||||
|
||||
// Parse measurements
|
||||
double bearing, range, bearing_std, range_std;
|
||||
|
||||
// A bearing-range measurement
|
||||
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 += 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);
|
||||
}
|
||||
}
|
||||
|
||||
// A landmark measurement, TODO Frank says: don't know why is converted to bearing-range
|
||||
if (tag == "LANDMARK") {
|
||||
int id1, id2;
|
||||
double lmx, lmy;
|
||||
double v1, v2, v3;
|
||||
|
||||
is >> id1 >> id2 >> lmx >> lmy >> v1 >> v2 >> v3;
|
||||
|
||||
// Convert x,y to bearing,range
|
||||
double bearing = std::atan2(lmy, lmx);
|
||||
double range = std::sqrt(lmx*lmx + lmy*lmy);
|
||||
bearing = std::atan2(lmy, lmx);
|
||||
range = std::sqrt(lmx*lmx + lmy*lmy);
|
||||
|
||||
// In our experience, the x-y covariance on landmark sightings is not very good, so assume
|
||||
// that it describes the uncertainty at a range of 10m, and convert that to bearing/range
|
||||
// uncertainty.
|
||||
SharedDiagonal measurementNoise;
|
||||
// it describes the uncertainty at a range of 10m, and convert that to bearing/range uncertainty.
|
||||
if(std::abs(v1 - v3) < 1e-4)
|
||||
{
|
||||
double rangeVar = v1;
|
||||
double bearingVar = v1 / 10.0;
|
||||
measurementNoise = noiseModel::Diagonal::Sigmas((Vector(2) << bearingVar, rangeVar));
|
||||
bearing_std = sqrt(v1 / 10.0);
|
||||
range_std = sqrt(v1);
|
||||
}
|
||||
else
|
||||
{
|
||||
bearing_std = 1;
|
||||
range_std = 1;
|
||||
if(!haveLandmark) {
|
||||
cout << "Warning: load2D is a very simple dataset loader and is ignoring the\n"
|
||||
"non-uniform covariance on LANDMARK measurements in this file." << endl;
|
||||
haveLandmark = true;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// Do some common stuff for bearing-range measurements
|
||||
if (tag == "LANDMARK" || tag == "BR") {
|
||||
|
||||
// optional filter
|
||||
if (maxID && id1 >= maxID)
|
||||
continue;
|
||||
|
||||
// Create noise model
|
||||
noiseModel::Diagonal::shared_ptr measurementNoise =
|
||||
noiseModel::Diagonal::Sigmas((Vector(2) << bearing_std, range_std));
|
||||
|
||||
// Add to graph
|
||||
*graph += BearingRangeFactor<Pose2, Point2>(id1, L(id2), bearing, range, measurementNoise);
|
||||
*graph += BearingRangeFactor<Pose2, Point2>(id1, L(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(L(id2))) {
|
||||
Pose2 pose = initial->at<Pose2>(id1);
|
||||
Point2 local(cos(bearing)*range,sin(bearing)*range);
|
||||
Point2 global = pose.transform_from(local);
|
||||
initial->insert(L(id2), global);
|
||||
}
|
||||
}
|
||||
is.ignore(LINESIZE, '\n');
|
||||
}
|
||||
|
||||
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, initial);
|
||||
}
|
||||
|
|
Loading…
Reference in New Issue