diff --git a/examples/ISAM2Example_SmartFactor.cpp b/examples/ISAM2Example_SmartFactor.cpp index 8435cc86e..77e29b8ff 100644 --- a/examples/ISAM2Example_SmartFactor.cpp +++ b/examples/ISAM2Example_SmartFactor.cpp @@ -1,5 +1,8 @@ -#include -#include +/** + * @file ISAM2Example_SmartFactor.cpp + * @brief test of iSAM with smart factors, led to bitbucket issue #367 + * @author Alexander (pumaking on BitBucket) + */ #include #include @@ -7,7 +10,13 @@ #include +#include +#include + +using namespace std; using namespace gtsam; +using symbol_shorthand::P; +using symbol_shorthand::X; // Make the typename short so it looks much cleaner typedef SmartProjectionPoseFactor SmartFactor; @@ -15,11 +24,12 @@ typedef SmartProjectionPoseFactor SmartFactor; int main(int argc, char* argv[]) { Cal3_S2::shared_ptr K(new Cal3_S2(50.0, 50.0, 0.0, 50.0, 50.0)); - noiseModel::Isotropic::shared_ptr measurement_noise = noiseModel::Isotropic::Sigma(2, 1.0); // one pixel in u and v + auto measurementNoise = + noiseModel::Isotropic::Sigma(2, 1.0); // one pixel in u and v - noiseModel::Diagonal::shared_ptr noise = noiseModel::Diagonal::Sigmas( - (Vector(6) << Vector3::Constant(0.3), Vector3::Constant(0.1)).finished()); - noiseModel::Isotropic::shared_ptr large_noise = noiseModel::Isotropic::Sigma(6, 100); + Vector6 sigmas; + sigmas << Vector3::Constant(0.3), Vector3::Constant(0.1); + auto noise = noiseModel::Diagonal::Sigmas(sigmas); ISAM2Params parameters; parameters.relinearizeThreshold = 0.01; @@ -31,7 +41,7 @@ int main(int argc, char* argv[]) { // Create a factor graph NonlinearFactorGraph graph; - Values initial_estimate; + Values initialEstimate; Point3 point(0.0, 0.0, 1.0); @@ -44,62 +54,61 @@ int main(int argc, char* argv[]) { Pose3 pose4(Rot3(), Point3(0.0, 0.5, 0.0)); Pose3 pose5(Rot3(), Point3(0.0, 0.6, 0.0)); - std::vector pose_list = { pose1, pose2, pose3, pose4, pose5 }; + vector poses = {pose1, pose2, pose3, pose4, pose5}; - SmartFactor::shared_ptr smart_factor(new SmartFactor(measurement_noise, K)); - graph.push_back(smart_factor); + SmartFactor::shared_ptr smartFactor(new SmartFactor(measurementNoise, K)); + graph.push_back(smartFactor); - graph.emplace_shared>(0, pose_list[0], noise); - initial_estimate.insert(0, pose_list[0].compose(delta)); - smart_factor->add(PinholePose(pose_list[0], K).project(point), 0); + graph.emplace_shared>(X(0), poses[0], noise); + initialEstimate.insert(X(0), poses[0].compose(delta)); + smartFactor->add(PinholePose(poses[0], K).project(point), X(0)); - for (int i = 1; i < 5; i++) { - PinholePose camera(pose_list[i], K); + for (size_t i = 1; i < 5; i++) { + cout << "****************************************************" << endl; + cout << "i = " << i << endl; + + // Add measurement to smart factor + PinholePose camera(poses[i], K); Point2 measurement = camera.project(point); + smartFactor->add(measurement, X(i)); + cout << "Measurement " << i << "" << measurement << endl; - std::cout << "****************************************************" << std::endl; - std::cout << "i = " << i << std::endl; - std::cout << "Measurement " << i << " is " << measurement << std::endl; - - graph.emplace_shared>(i, pose_list[i], noise); - //graph.emplace_shared>(i - 1, i, Pose3(), large_noise); - initial_estimate.insert(i, pose_list[i].compose(delta)); - smart_factor->add(measurement, i); - - ISAM2Result result = isam.update(graph, initial_estimate); - graph.resize(0); - initial_estimate.clear(); + // Add prior on new pose + graph.emplace_shared>(X(i), poses[i], noise); + initialEstimate.insert(X(i), poses[i].compose(delta)); + // Update iSAM2 + ISAM2Result result = isam.update(graph, initialEstimate); result.print(); - const auto& var_map = (*(result.detail)).variableStatus; - - std::cout << "Detailed results:" << std::endl; - for (int j = 0; j < 3; j++) { - if (var_map.exists(j)) { - std::cout << j << " is reeliminated: " << var_map.at(j).isReeliminated << std::endl; - std::cout << j << " is relinearized above thresh: " << var_map.at(j).isAboveRelinThreshold << std::endl; - std::cout << j << " is relinearized involved: " << var_map.at(j).isRelinearizeInvolved << std::endl; - std::cout << j << " is relinearized: " << var_map.at(j).isRelinearized << std::endl; - std::cout << j << " is observed: " << var_map.at(j).isObserved << std::endl; - std::cout << j << " is new: " << var_map.at(j).isNew << std::endl; - std::cout << j << " is in the root clique: " << var_map.at(j).inRootClique << std::endl; - } - else { - std::cout << j << " does not exist in the detailed results map." << std::endl; - } + cout << "Detailed results:" << endl; + for (auto keyedStatus : result.detail->variableStatus) { + const auto& status = keyedStatus.second; + cout << keyedStatus.first << " {" << endl; + cout << "reeliminated: " << status.isReeliminated << endl; + cout << "relinearized above thresh: " << status.isAboveRelinThreshold + << endl; + cout << "relinearized involved: " << status.isRelinearizeInvolved << endl; + cout << "relinearized: " << status.isRelinearized << endl; + cout << "observed: " << status.isObserved << endl; + cout << "new: " << status.isNew << endl; + cout << "in the root clique: " << status.inRootClique << endl; + cout << "}" << endl; } - Values current_estimate = isam.calculateEstimate(); - current_estimate.print("Current estimate: "); + Values currentEstimate = isam.calculateEstimate(); + currentEstimate.print("Current estimate: "); - boost::optional point_res = smart_factor->point(current_estimate); - if (point_res) { - std::cout << *point_res << std::endl; - } - else { - std::cout << "Point is degenerate." << std::endl; + boost::optional pointEstimate = smartFactor->point(currentEstimate); + if (pointEstimate) { + cout << *pointEstimate << endl; + } else { + cout << "Point degenerate." << endl; } + + // Reset graph and initial estimate for next iteration + graph.resize(0); + initialEstimate.clear(); } return 0;