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

43
examples/SolverComparer.cpp
View File

@ -166,10 +166,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);
}
}
@ -189,7 +194,6 @@ int main(int argc, char *argv[]) {
/* ************************************************************************* */
void runIncremental()
{
#if 0
ISAM2 isam2;
// Look for the first measurement to use
@ -264,21 +268,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 =
@ -350,6 +356,8 @@ void runIncremental()
}
}
tictoc_print_();
// Compute marginals
//try {
// Marginals marginals(graph, values);
@ -386,7 +394,6 @@ void runIncremental()
// cout << e.what() << endl;
//}
//tictoc_print_();
#endif
}
/* ************************************************************************* */