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:26:29 +00:00
parent d4e4705fcd
commit 4c59e13f94
1 changed files with 25 additions and 18 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

15
examples/SolverComparer.cpp
View File

@ -166,10 +166,15 @@ int main(int argc, char *argv[]) {
if(!datasetName.empty()) if(!datasetName.empty())
{ {
cout << "Loading dataset " << datasetName << endl; cout << "Loading dataset " << datasetName << endl;
try {
string datasetFile = findExampleDataFile(datasetName); string datasetFile = findExampleDataFile(datasetName);
std::pair<NonlinearFactorGraph::shared_ptr, Values::shared_ptr> data = std::pair<NonlinearFactorGraph::shared_ptr, Values::shared_ptr> data =
load2D(datasetFile); load2D(datasetFile);
datasetMeasurements = *data.first; datasetMeasurements = *data.first;
} catch(std::exception& e) {
cout << e.what() << endl;
exit(1);
}
} }
@ -189,7 +194,6 @@ int main(int argc, char *argv[]) {
/* ************************************************************************* */ /* ************************************************************************* */
void runIncremental() void runIncremental()
{ {
#if 0
ISAM2 isam2; ISAM2 isam2;
// Look for the first measurement to use // Look for the first measurement to use
@ -264,21 +268,23 @@ void runIncremental()
// Initialize the new variable // Initialize the new variable
if(measurement->key1() > measurement->key2()) { if(measurement->key1() > measurement->key2()) {
if(!newVariables.exists(measurement->key1())) { // Only need to check newVariables since loop closures come after odometry
if(step == 1) if(step == 1)
newVariables.insert(measurement->key1(), measurement->measured().inverse()); newVariables.insert(measurement->key1(), measurement->measured().inverse());
else { else {
Pose prevPose = isam2.calculateEstimate<Pose>(measurement->key2()); Pose prevPose = isam2.calculateEstimate<Pose>(measurement->key2());
newVariables.insert(measurement->key1(), prevPose * measurement->measured().inverse()); newVariables.insert(measurement->key1(), prevPose * measurement->measured().inverse());
} }
// cout << "Initializing " << step << endl; }
} else { } else {
if(!newVariables.exists(measurement->key1())) { // Only need to check newVariables since loop closures come after odometry
if(step == 1) if(step == 1)
newVariables.insert(measurement->key2(), measurement->measured()); newVariables.insert(measurement->key2(), measurement->measured());
else { else {
Pose prevPose = isam2.calculateEstimate<Pose>(measurement->key1()); Pose prevPose = isam2.calculateEstimate<Pose>(measurement->key1());
newVariables.insert(measurement->key2(), prevPose * measurement->measured()); 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 =
@ -350,6 +356,8 @@ void runIncremental()
} }
} }
tictoc_print_();
// Compute marginals // Compute marginals
//try { //try {
// Marginals marginals(graph, values); // Marginals marginals(graph, values);
@ -386,7 +394,6 @@ void runIncremental()
// cout << e.what() << endl; // cout << e.what() << endl;
//} //}
//tictoc_print_(); //tictoc_print_();
#endif
} }
/* ************************************************************************* */ /* ************************************************************************* */