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()) if(!datasetName.empty())
{ {
cout << "Loading dataset " << datasetName << endl; cout << "Loading dataset " << datasetName << endl;
string datasetFile = findExampleDataFile(datasetName); try {
std::pair<NonlinearFactorGraph::shared_ptr, Values::shared_ptr> data = string datasetFile = findExampleDataFile(datasetName);
load2D(datasetFile); std::pair<NonlinearFactorGraph::shared_ptr, Values::shared_ptr> data =
datasetMeasurements = *data.first; 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 // Initialize the new variable
if(measurement->key1() > measurement->key2()) { if(measurement->key1() > measurement->key2()) {
if(step == 1) if(!newVariables.exists(measurement->key1())) { // Only need to check newVariables since loop closures come after odometry
newVariables.insert(measurement->key1(), measurement->measured().inverse()); if(step == 1)
else { newVariables.insert(measurement->key1(), measurement->measured().inverse());
Pose prevPose = isam2.calculateEstimate<Pose>(measurement->key2()); else {
newVariables.insert(measurement->key1(), prevPose * measurement->measured().inverse()); Pose prevPose = isam2.calculateEstimate<Pose>(measurement->key2());
newVariables.insert(measurement->key1(), prevPose * measurement->measured().inverse());
}
} }
// cout << "Initializing " << step << endl;
} else { } else {
if(step == 1) if(!newVariables.exists(measurement->key1())) { // Only need to check newVariables since loop closures come after odometry
newVariables.insert(measurement->key2(), measurement->measured()); if(step == 1)
else { newVariables.insert(measurement->key2(), measurement->measured());
Pose prevPose = isam2.calculateEstimate<Pose>(measurement->key1()); else {
newVariables.insert(measurement->key2(), prevPose * measurement->measured()); 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 = else if(BearingRangeFactor<Pose, Point2>::shared_ptr measurement =
@ -348,6 +355,8 @@ void runIncremental()
} }
} }
tictoc_print_();
// Compute marginals // Compute marginals
//try { //try {
// Marginals marginals(graph, values); // Marginals marginals(graph, values);