12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Some more cleanup

release/4.3a0
Frank Dellaert 2018-09-29 17:31:46 -04:00
parent 315ea79e21
commit 05d5179bc3
1 changed files with 18 additions and 13 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

31
examples/ISAM2Example_SmartFactor.cpp
View File

@ -5,11 +5,10 @@
*/ */
#include <gtsam/geometry/SimpleCamera.h> #include <gtsam/geometry/SimpleCamera.h>
#include <gtsam/nonlinear/ISAM2.h>
#include <gtsam/slam/BetweenFactor.h> #include <gtsam/slam/BetweenFactor.h>
#include <gtsam/slam/SmartProjectionPoseFactor.h> #include <gtsam/slam/SmartProjectionPoseFactor.h>
#include <gtsam/nonlinear/ISAM2.h>
#include <iostream> #include <iostream>
#include <vector> #include <vector>
@ -56,27 +55,32 @@ int main(int argc, char* argv[]) {
vector<Pose3> poses = {pose1, pose2, pose3, pose4, pose5}; vector<Pose3> poses = {pose1, pose2, pose3, pose4, pose5};
SmartFactor::shared_ptr smartFactor(new SmartFactor(measurementNoise, K)); // Add first pose
graph.push_back(smartFactor);
graph.emplace_shared<PriorFactor<Pose3>>(X(0), poses[0], noise); graph.emplace_shared<PriorFactor<Pose3>>(X(0), poses[0], noise);
initialEstimate.insert(X(0), poses[0].compose(delta)); initialEstimate.insert(X(0), poses[0].compose(delta));
smartFactor->add(PinholePose<Cal3_S2>(poses[0], K).project(point), X(0));
// Create smart factor with measurement from first pose only
SmartFactor::shared_ptr smartFactor(new SmartFactor(measurementNoise, K));
smartFactor->add(PinholePose<Cal3_S2>(poses[0], K).project(point), X(0));
graph.push_back(smartFactor);
// loop over remaining poses
for (size_t i = 1; i < 5; i++) { for (size_t i = 1; i < 5; i++) {
cout << "****************************************************" << endl; cout << "****************************************************" << endl;
cout << "i = " << i << endl; cout << "i = " << i << endl;
// Add measurement to smart factor
PinholePose<Cal3_S2> camera(poses[i], K);
Point2 measurement = camera.project(point);
smartFactor->add(measurement, X(i));
cout << "Measurement " << i << "" << measurement << endl;
// Add prior on new pose // Add prior on new pose
graph.emplace_shared<PriorFactor<Pose3>>(X(i), poses[i], noise); graph.emplace_shared<PriorFactor<Pose3>>(X(i), poses[i], noise);
initialEstimate.insert(X(i), poses[i].compose(delta)); initialEstimate.insert(X(i), poses[i].compose(delta));
// "Simulate" measurement from this pose
PinholePose<Cal3_S2> camera(poses[i], K);
Point2 measurement = camera.project(point);
cout << "Measurement " << i << "" << measurement << endl;
// Add measurement to smart factor
smartFactor->add(measurement, X(i));
// Update iSAM2 // Update iSAM2
ISAM2Result result = isam.update(graph, initialEstimate); ISAM2Result result = isam.update(graph, initialEstimate);
result.print(); result.print();
@ -84,7 +88,8 @@ int main(int argc, char* argv[]) {
cout << "Detailed results:" << endl; cout << "Detailed results:" << endl;
for (auto keyedStatus : result.detail->variableStatus) { for (auto keyedStatus : result.detail->variableStatus) {
const auto& status = keyedStatus.second; const auto& status = keyedStatus.second;
cout << keyedStatus.first << " {" << endl; PrintKey(keyedStatus.first);
cout << " {" << endl;
cout << "reeliminated: " << status.isReeliminated << endl; cout << "reeliminated: " << status.isReeliminated << endl;
cout << "relinearized above thresh: " << status.isAboveRelinThreshold cout << "relinearized above thresh: " << status.isAboveRelinThreshold
<< endl; << endl;