ISAM2Example_SmartFactor.cpp
Go to the documentation of this file.
1 
12 
13 #include <iostream>
14 #include <vector>
15 
16 using namespace std;
17 using namespace gtsam;
20 
21 // Make the typename short so it looks much cleaner
23 
24 int main(int argc, char* argv[]) {
25  Cal3_S2::shared_ptr K(new Cal3_S2(50.0, 50.0, 0.0, 50.0, 50.0));
26 
27  auto measurementNoise =
28  noiseModel::Isotropic::Sigma(2, 1.0); // one pixel in u and v
29 
30  Vector6 sigmas;
31  sigmas << Vector3::Constant(0.1), Vector3::Constant(0.3);
32  auto noise = noiseModel::Diagonal::Sigmas(sigmas);
33 
35  parameters.relinearizeThreshold = 0.01;
36  parameters.relinearizeSkip = 1;
37  parameters.cacheLinearizedFactors = false;
38  parameters.enableDetailedResults = true;
39  parameters.print();
41 
42  // Create a factor graph
44  Values initialEstimate;
45 
46  Point3 point(0.0, 0.0, 1.0);
47 
48  // Intentionally initialize the variables off from the ground truth
49  Pose3 delta(Rot3::Rodrigues(0.0, 0.0, 0.0), Point3(0.05, -0.10, 0.20));
50 
51  Pose3 pose1(Rot3(), Point3(0.0, 0.0, 0.0));
52  Pose3 pose2(Rot3(), Point3(0.0, 0.2, 0.0));
53  Pose3 pose3(Rot3(), Point3(0.0, 0.4, 0.0));
54  Pose3 pose4(Rot3(), Point3(0.0, 0.5, 0.0));
55  Pose3 pose5(Rot3(), Point3(0.0, 0.6, 0.0));
56 
57  vector<Pose3> poses = {pose1, pose2, pose3, pose4, pose5};
58 
59  // Add first pose
60  graph.addPrior(X(0), poses[0], noise);
61  initialEstimate.insert(X(0), poses[0].compose(delta));
62 
63  // Create smart factor with measurement from first pose only
64  SmartFactor::shared_ptr smartFactor(new SmartFactor(measurementNoise, K));
65  smartFactor->add(PinholePose<Cal3_S2>(poses[0], K).project(point), X(0));
66  graph.push_back(smartFactor);
67 
68  // loop over remaining poses
69  for (size_t i = 1; i < 5; i++) {
70  cout << "****************************************************" << endl;
71  cout << "i = " << i << endl;
72 
73  // Add prior on new pose
74  graph.addPrior(X(i), poses[i], noise);
75  initialEstimate.insert(X(i), poses[i].compose(delta));
76 
77  // "Simulate" measurement from this pose
78  PinholePose<Cal3_S2> camera(poses[i], K);
80  cout << "Measurement " << i << "" << measurement << endl;
81 
82  // Add measurement to smart factor
83  smartFactor->add(measurement, X(i));
84 
85  // Update iSAM2
86  ISAM2Result result = isam.update(graph, initialEstimate);
87  result.print();
88 
89  cout << "Detailed results:" << endl;
90  for (auto& [key, status] : result.detail->variableStatus) {
91  PrintKey(key);
92  cout << " {" << endl;
93  cout << "reeliminated: " << status.isReeliminated << endl;
94  cout << "relinearized above thresh: " << status.isAboveRelinThreshold
95  << endl;
96  cout << "relinearized involved: " << status.isRelinearizeInvolved << endl;
97  cout << "relinearized: " << status.isRelinearized << endl;
98  cout << "observed: " << status.isObserved << endl;
99  cout << "new: " << status.isNew << endl;
100  cout << "in the root clique: " << status.inRootClique << endl;
101  cout << "}" << endl;
102  }
103 
104  Values currentEstimate = isam.calculateEstimate();
105  currentEstimate.print("Current estimate: ");
106 
107  auto pointEstimate = smartFactor->point(currentEstimate);
108  if (pointEstimate) {
109  cout << *pointEstimate << endl;
110  } else {
111  cout << "Point degenerate." << endl;
112  }
113 
114  // Reset graph and initial estimate for next iteration
115  graph.resize(0);
116  initialEstimate.clear();
117  }
118 
119  return 0;
120 }
gtsam::ConjugateGradientParameters::print
void print() const
Definition: ConjugateGradientSolver.h:89
gtsam::ISAM2Params
Definition: ISAM2Params.h:136
main
int main(int argc, char *argv[])
Definition: ISAM2Example_SmartFactor.cpp:24
gtsam::ISAM2
Definition: ISAM2.h:45
Cal3_S2.h
The most common 5DOF 3D->2D calibration.
test_constructor::sigmas
Vector1 sigmas
Definition: testHybridNonlinearFactor.cpp:52
pose3
static Pose3 pose3(rt3, pt3)
simple::pose2
static Pose3 pose2
Definition: testInitializePose3.cpp:58
X
#define X
Definition: icosphere.cpp:20
result
Values result
Definition: OdometryOptimize.cpp:8
gtsam::NonlinearISAM::update
void update(const NonlinearFactorGraph &newFactors, const Values &initialValues)
Definition: NonlinearISAM.cpp:35
gtsam_unstable.tests.test_ProjectionFactorRollingShutter.point
point
Definition: test_ProjectionFactorRollingShutter.py:25
gtsam::FactorGraph::resize
virtual void resize(size_t size)
Definition: FactorGraph.h:367
gtsam::PrintKey
void PrintKey(Key key, const string &s, const KeyFormatter &keyFormatter)
Utility function to print one key with optional prefix.
Definition: Key.cpp:44
gtsam.examples.PlanarManipulatorExample.delta
def delta(g0, g1)
Definition: PlanarManipulatorExample.py:45
BetweenFactor.h
gtsam::Rot3
Rot3 is a 3D rotation represented as a rotation matrix if the preprocessor symbol GTSAM_USE_QUATERNIO...
Definition: Rot3.h:58
gtsam::Pose3
Definition: Pose3.h:37
parameters
static ConjugateGradientParameters parameters
Definition: testIterative.cpp:33
gtsam::SmartProjectionPoseFactor
Definition: SmartProjectionPoseFactor.h:45
isam
NonlinearISAM isam(relinearizeInterval)
gtsam::NonlinearFactorGraph
Definition: NonlinearFactorGraph.h:55
gtsam::NonlinearFactorGraph::addPrior
void addPrior(Key key, const T &prior, const SharedNoiseModel &model=nullptr)
Definition: NonlinearFactorGraph.h:199
PinholeCamera.h
Base class for all pinhole cameras.
gtsam::Point2
Vector2 Point2
Definition: Point2.h:32
gtsam::Cal3_S2::shared_ptr
std::shared_ptr< Cal3_S2 > shared_ptr
Definition: Cal3_S2.h:39
SmartFactor
SmartProjectionPoseFactor< Cal3_S2 > SmartFactor
Definition: ISAM2Example_SmartFactor.cpp:22
SmartProjectionPoseFactor.h
Smart factor on poses, assuming camera calibration is fixed.
key
const gtsam::Symbol key('X', 0)
ISAM2.h
Incremental update functionality (ISAM2) for BayesTree, with fluid relinearization.
gtsam::Values::print
void print(const std::string &str="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const
Definition: Values.cpp:66
gtsam
traits
Definition: SFMdata.h:40
pose1
static const Pose3 pose1(Rot3(), Point3(0, 1, 0.5))
K
#define K
Definition: igam.h:8
project
static Point2 project(const Pose3 &pose, const Unit3 &pointAtInfinity, const Cal3_S2::shared_ptr &cal)
Definition: testPinholePose.cpp:188
gtsam::FactorGraph::push_back
IsDerived< DERIVEDFACTOR > push_back(std::shared_ptr< DERIVEDFACTOR > factor)
Add a factor directly using a shared_ptr.
Definition: FactorGraph.h:147
gtsam::Values
Definition: Values.h:65
gtsam::testing::compose
T compose(const T &t1, const T &t2)
Definition: lieProxies.h:39
std
Definition: BFloat16.h:88
gtsam::Values::clear
void clear()
Definition: Values.h:347
gtsam::Values::insert
void insert(Key j, const Value &val)
Definition: Values.cpp:155
P
static double P[]
Definition: ellpe.c:68
gtsam::Cal3_S2
The most common 5DOF 3D->2D calibration.
Definition: Cal3_S2.h:34
gtsam::Point3
Vector3 Point3
Definition: Point3.h:38
gtsam::ISAM2Result
Definition: ISAM2Result.h:39
gtsam::SmartProjectionPoseFactor::shared_ptr
std::shared_ptr< This > shared_ptr
shorthand for a smart pointer to a factor
Definition: SmartProjectionPoseFactor.h:59
camera
static const CalibratedCamera camera(kDefaultPose)
gtsam::PinholePose
Definition: PinholePose.h:239
graph
NonlinearFactorGraph graph
Definition: doc/Code/OdometryExample.cpp:2
i
int i
Definition: BiCGSTAB_step_by_step.cpp:9
measurement
static Point2 measurement(323.0, 240.0)
gtsam::CalibratedCamera::project
Point2 project(const Point3 &point, OptionalJacobian< 2, 6 > Dcamera={}, OptionalJacobian< 2, 3 > Dpoint={}) const
Definition: CalibratedCamera.cpp:188


gtsam
Author(s):
autogenerated on Sat Nov 16 2024 04:02:33