diff --git a/examples/ISAM2Example_SmartFactor.cpp b/examples/ISAM2Example_SmartFactor.cpp new file mode 100644 index 000000000..8435cc86e --- /dev/null +++ b/examples/ISAM2Example_SmartFactor.cpp @@ -0,0 +1,106 @@ +#include +#include + +#include +#include +#include + +#include + +using namespace gtsam; + +// Make the typename short so it looks much cleaner +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 + + 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); + + ISAM2Params parameters; + parameters.relinearizeThreshold = 0.01; + parameters.relinearizeSkip = 1; + parameters.cacheLinearizedFactors = false; + parameters.enableDetailedResults = true; + parameters.print(); + ISAM2 isam(parameters); + + // Create a factor graph + NonlinearFactorGraph graph; + Values initial_estimate; + + Point3 point(0.0, 0.0, 1.0); + + // Intentionally initialize the variables off from the ground truth + Pose3 delta(Rot3::Rodrigues(0.0, 0.0, 0.0), Point3(0.05, -0.10, 0.20)); + + Pose3 pose1(Rot3(), Point3(0.0, 0.0, 0.0)); + Pose3 pose2(Rot3(), Point3(0.0, 0.2, 0.0)); + Pose3 pose3(Rot3(), Point3(0.0, 0.4, 0.0)); + 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 }; + + SmartFactor::shared_ptr smart_factor(new SmartFactor(measurement_noise, K)); + graph.push_back(smart_factor); + + 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); + + for (int i = 1; i < 5; i++) { + PinholePose camera(pose_list[i], K); + Point2 measurement = camera.project(point); + + 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(); + + 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; + } + } + + Values current_estimate = isam.calculateEstimate(); + current_estimate.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; + } + } + + return 0; +}