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

Reenabled ISAM2 in SolverComparer

release/4.3a0
Richard Roberts 2013-08-11 19:29:01 +00:00
parent f41d4b879b
commit 18a72718aa
1 changed files with 25 additions and 16 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

41
examples/SolverComparer.cpp
View File

@ -165,10 +165,15 @@ int main(int argc, char *argv[]) {
if(!datasetName.empty())
{
cout << "Loading dataset " << datasetName << endl;
string datasetFile = findExampleDataFile(datasetName);
std::pair<NonlinearFactorGraph::shared_ptr, Values::shared_ptr> data =
load2D(datasetFile);
datasetMeasurements = *data.first;
try {
string datasetFile = findExampleDataFile(datasetName);
std::pair<NonlinearFactorGraph::shared_ptr, Values::shared_ptr> data =
load2D(datasetFile);
datasetMeasurements = *data.first;
} catch(std::exception& e) {
cout << e.what() << endl;
exit(1);
}
}
@ -262,21 +267,23 @@ void runIncremental()
// Initialize the new variable
if(measurement->key1() > measurement->key2()) {
if(step == 1)
newVariables.insert(measurement->key1(), measurement->measured().inverse());
else {
Pose prevPose = isam2.calculateEstimate<Pose>(measurement->key2());
newVariables.insert(measurement->key1(), prevPose * measurement->measured().inverse());
if(!newVariables.exists(measurement->key1())) { // Only need to check newVariables since loop closures come after odometry
if(step == 1)
newVariables.insert(measurement->key1(), measurement->measured().inverse());
else {
Pose prevPose = isam2.calculateEstimate<Pose>(measurement->key2());
newVariables.insert(measurement->key1(), prevPose * measurement->measured().inverse());
}
}
// cout << "Initializing " << step << endl;
} else {
if(step == 1)
newVariables.insert(measurement->key2(), measurement->measured());
else {
Pose prevPose = isam2.calculateEstimate<Pose>(measurement->key1());
newVariables.insert(measurement->key2(), prevPose * measurement->measured());
if(!newVariables.exists(measurement->key1())) { // Only need to check newVariables since loop closures come after odometry
if(step == 1)
newVariables.insert(measurement->key2(), measurement->measured());
else {
Pose prevPose = isam2.calculateEstimate<Pose>(measurement->key1());
newVariables.insert(measurement->key2(), prevPose * measurement->measured());
}
}
// cout << "Initializing " << step << endl;
}
}
else if(BearingRangeFactor<Pose, Point2>::shared_ptr measurement =
@ -348,6 +355,8 @@ void runIncremental()
}
}
tictoc_print_();
// Compute marginals
//try {
// Marginals marginals(graph, values);