testSmartStereoFactor_iSAM2.cpp
Go to the documentation of this file.
1 /* ----------------------------------------------------------------------------
2  * GTSAM Copyright 2010, Georgia Tech Research Corporation,
3  * Atlanta, Georgia 30332-0415
4  * All Rights Reserved
5  * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
6  * See LICENSE for the license information
7  * -------------------------------------------------------------------------- */
8 
18 #include <CppUnitLite/TestHarness.h>
19 
20 #include <gtsam/base/debug.h>
21 #include <gtsam/nonlinear/ISAM2.h>
22 #include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
23 #include <gtsam_unstable/slam/SmartStereoProjectionPoseFactor.h>
24 
25 #include <array>
26 #include <fstream>
27 #include <iostream>
28 #include <sstream>
29 #include <string>
30 #include <vector>
31 
32 // Set to 1 to enable verbose output of intermediary results
33 #define TEST_VERBOSE_OUTPUT 0
34 
35 #if TEST_VERBOSE_OUTPUT
36 #define TEST_COUT(ARGS_) std::cout << ARGS_
37 #else
38 #define TEST_COUT(ARGS_) void(0)
39 #endif
40 
41 // Tolerance for ground-truth pose comparison:
42 static const double tol = 1e-3;
43 
44 // Synthetic dataset generated with rwt
45 // (https://github.com/jlblancoc/recursive-world-toolkit)
46 // Camera parameters
47 const double fx = 200.0;
48 const double fy = 150.0;
49 const double cx = 512.0;
50 const double cy = 384.0;
51 const double baseline = 0.2; // meters
52 
53 using timestep_t = std::size_t;
54 using lm_id_t = int;
55 
56 struct stereo_meas_t {
57  stereo_meas_t(lm_id_t id, double lu, double ru, double v_lr)
58  : lm_id{id}, left_u{lu}, right_u{ru}, v{v_lr} {}
59 
60  lm_id_t lm_id{-1}; // landmark id
61  double left_u{0}, right_u{0}, v{0};
62 };
63 
64 static std::map<timestep_t, std::vector<stereo_meas_t>> dataset = {
65  {0,
66  {{0, 911.99993896, 712.00000000, 384.0},
67  {159, 311.99996948, 211.99996948, 384.0},
68  {3, 378.66665649, 312.00000000, 384.0},
69  {2, 645.33331299, 578.66662598, 384.0},
70  {157, 111.99994659, 11.99993896, 384.0},
71  {4, 578.66662598, 545.33331299, 384.0},
72  {5, 445.33331299, 412.00000000, 384.0},
73  {6, 562.00000000, 537.00000000, 384.0}}},
74  {1,
75  {{0, 1022.06353760, 762.57519531, 384.0},
76  {159, 288.30487061, 177.80273438, 384.0},
77  {2, 655.30645752, 583.12127686, 384.0},
78  {3, 368.60937500, 297.43176270, 384.0},
79  {4, 581.82666016, 547.16766357, 384.0},
80  {5, 443.66183472, 409.23681641, 384.0},
81  {6, 564.35980225, 538.62115479, 384.0},
82  {7, 461.66418457, 436.05477905, 384.0},
83  {8, 550.32220459, 531.75256348, 384.0},
84  {9, 476.17767334, 457.67541504, 384.0}}},
85  {2,
86  {{159, 257.97128296, 134.26287842, 384.0},
87  {2, 666.87255859, 588.07275391, 384.0},
88  {3, 356.53823853, 280.10061646, 384.0},
89  {4, 585.10949707, 548.99212646, 384.0},
90  {5, 441.66403198, 406.05108643, 384.0},
91  {6, 566.75402832, 540.21868896, 384.0},
92  {7, 461.16207886, 434.90002441, 384.0},
93  {8, 552.28387451, 533.30230713, 384.0},
94  {9, 476.63549805, 457.79418945, 384.0},
95  {10, 546.48394775, 530.53009033, 384.0}}},
96  {3,
97  {{159, 218.10592651, 77.30914307, 384.0},
98  {2, 680.54644775, 593.68103027, 384.0},
99  {3, 341.92507935, 259.28231812, 384.0},
100  {4, 588.53289795, 550.80499268, 384.0},
101  {5, 439.29989624, 402.39105225, 384.0},
102  {6, 569.18627930, 541.78991699, 384.0},
103  {7, 460.47863770, 433.51678467, 384.0},
104  {8, 554.24902344, 534.82952881, 384.0},
105  {9, 477.00451660, 457.80438232, 384.0},
106  {10, 548.33770752, 532.07501221, 384.0},
107  {11, 483.58688354, 467.47830200, 384.0},
108  {12, 542.36785889, 529.29321289, 384.0}}},
109  {4,
110  {{2, 697.09454346, 600.18432617, 384.0},
111  {3, 324.03643799, 233.97094727, 384.0},
112  {4, 592.11877441, 552.60449219, 384.0},
113  {5, 436.52197266, 398.19531250, 384.0},
114  {6, 571.66101074, 543.33209229, 384.0},
115  {7, 459.59658813, 431.88333130, 384.0},
116  {8, 556.21801758, 536.33258057, 384.0},
117  {9, 477.27893066, 457.69882202, 384.0},
118  {10, 550.18920898, 533.60003662, 384.0},
119  {11, 484.24472046, 467.86862183, 384.0},
120  {12, 544.14727783, 530.86157227, 384.0},
121  {13, 491.26141357, 478.11267090, 384.0},
122  {14, 541.29949951, 529.57086182, 384.0},
123  {15, 494.58111572, 482.95935059, 384.0}}}};
124 
125 // clang-format off
126 /*
127 % Ground truth path of the SENSOR, and the ROBOT
128 % STEP X Y Z QR QX QY QZ | X Y Z QR QX QY QZ
129  ----------------------------------------------------------------------------------------------------------------------------------------
130  0 0.000000 0.000000 0.000000 0.500000 -0.500000 0.500000 -0.500000 0.000000 0.000000 0.000000 1.000000 0.000000 0.000000 0.000000
131  1 0.042019 -0.008403 0.000000 0.502446 -0.502446 0.497542 -0.497542 0.042019 -0.008403 0.000000 0.999988 0.000000 0.000000 0.004905
132  2 0.084783 -0.016953 0.000000 0.504879 -0.504879 0.495073 -0.495073 0.084783 -0.016953 0.000000 0.999952 0.000000 0.000000 0.009806
133  3 0.128305 -0.025648 0.000000 0.507299 -0.507299 0.492592 -0.492592 0.128305 -0.025648 0.000000 0.999892 0.000000 0.000000 0.014707
134  4 0.172605 -0.034490 0.000000 0.509709 -0.509709 0.490098 -0.490098 0.172605 -0.034490 0.000000 0.999808 0.000000 0.000000 0.019611
135 */
136 // clang-format on
137 
138 // Ground truth using camera pose = vehicle frame
139 // The table above uses:
140 // camera +x = vehicle -y
141 // camera +y = vehicle -z
142 // camera +z = vehicle +x
143 static const std::map<timestep_t, gtsam::Point3> gt_positions = {
144  {0, {0.000000, 0.000000, 0.0}},
145  {1, {0.042019, -0.008403, 0.0}},
146  {2, {0.084783, -0.016953, 0.0}},
147  {3, {0.128305, -0.025648, 0.0}},
148  {4, {0.172605, -0.034490, 0.0}}};
149 
150 // Batch version, to compare against iSAM2 solution.
151 TEST(testISAM2SmartFactor, Stereo_Batch) {
152  TEST_COUT("============ Running: Batch ============\n");
153 
154  using namespace gtsam;
155  using symbol_shorthand::V;
156  using symbol_shorthand::X;
157 
158  const auto K =
159  std::make_shared<Cal3_S2Stereo>(fx, fy, .0, cx, cy, baseline);
160 
161  // Pose prior - at identity
162  auto priorPoseNoise = noiseModel::Diagonal::Sigmas(
163  (Vector(6) << Vector3::Constant(0.2), Vector3::Constant(0.2)).finished());
164 
165  // Map: landmark_id => smart_factor_index inside iSAM2
166  std::map<lm_id_t, FactorIndex> lm2factor;
167 
168  // Storage of smart factors:
169  std::map<lm_id_t, SmartStereoProjectionPoseFactor::shared_ptr> smartFactors;
170 
171  NonlinearFactorGraph batch_graph;
172  Values batch_values;
173 
174  // Run one timestep at once:
175  for (const auto &entries : dataset) {
176  // 1) Process new observations:
177  // ------------------------------
178  const auto kf_id = entries.first;
179  const std::vector<stereo_meas_t> &obs = entries.second;
180 
181  for (const stereo_meas_t &stObs : obs) {
182  if (smartFactors.count(stObs.lm_id) == 0) {
183  auto noise = noiseModel::Isotropic::Sigma(3, 0.1);
184  SmartProjectionParams parm(HESSIAN, ZERO_ON_DEGENERACY);
185 
186  smartFactors[stObs.lm_id] =
187  std::make_shared<SmartStereoProjectionPoseFactor>(noise, parm);
188 
189  batch_graph.push_back(smartFactors[stObs.lm_id]);
190  }
191 
192  TEST_COUT("Adding stereo observation from KF #" << kf_id << " for LM #"
193  << stObs.lm_id << "\n");
194 
195  smartFactors[stObs.lm_id]->add(
196  StereoPoint2(stObs.left_u, stObs.right_u, stObs.v), X(kf_id), K);
197  }
198 
199  // prior, for the first keyframe:
200  if (kf_id == 0) {
201  batch_graph.addPrior(X(kf_id), Pose3::Identity(), priorPoseNoise);
202  }
203 
204  batch_values.insert(X(kf_id), Pose3::Identity());
205  }
206 
207  LevenbergMarquardtParams parameters;
208 #if TEST_VERBOSE_OUTPUT
209  parameters.verbosity = NonlinearOptimizerParams::LINEAR;
210  parameters.verbosityLM = LevenbergMarquardtParams::TRYDELTA;
211 #endif
212 
213  LevenbergMarquardtOptimizer lm(batch_graph, batch_values, parameters);
214 
215  Values finalEstimate = lm.optimize();
216 #if TEST_VERBOSE_OUTPUT
217  finalEstimate.print("LevMarq estimate:");
218 #endif
219 
220  // GT:
221  // camera +x = vehicle -y
222  // camera +y = vehicle -z
223  // camera +z = vehicle +x
224  for (const auto &gt : gt_positions) {
225  const Pose3 p = finalEstimate.at<Pose3>(X(gt.first));
226  EXPECT(assert_equal(p.x(), -gt.second.y(), tol));
227  EXPECT(assert_equal(p.y(), -gt.second.z(), tol));
228  EXPECT(assert_equal(p.z(), gt.second.x(), tol));
229  }
230 }
231 
232 TEST(testISAM2SmartFactor, Stereo_iSAM2) {
233  TEST_COUT("======= Running: iSAM2 ==========\n");
234 
235 #if TEST_VERBOSE_OUTPUT
236  SETDEBUG("ISAM2 update", true);
237  // SETDEBUG("ISAM2 update verbose",true);
238 #endif
239 
240  using namespace gtsam;
241  using symbol_shorthand::V;
242  using symbol_shorthand::X;
243 
244  const auto K =
245  std::make_shared<Cal3_S2Stereo>(fx, fy, .0, cx, cy, baseline);
246 
247  ISAM2Params parameters;
248  parameters.relinearizeThreshold = 0.01;
249  parameters.evaluateNonlinearError = true;
250 
251  // Do not cache smart factors:
252  parameters.cacheLinearizedFactors = false;
253 
254  // Important: must set relinearizeSkip=1 to additional calls to update() to
255  // have a real effect.
256  parameters.relinearizeSkip = 1;
257 
258  ISAM2 isam(parameters);
259 
260  // Pose prior - at identity
261  auto priorPoseNoise = noiseModel::Diagonal::Sigmas(
262  (Vector(6) << Vector3::Constant(0.2), Vector3::Constant(0.2)).finished());
263 
264  // Map: landmark_id => smart_factor_index inside iSAM2
265  std::map<lm_id_t, FactorIndex> lm2factor;
266 
267  // Storage of smart factors:
268  std::map<lm_id_t, SmartStereoProjectionPoseFactor::shared_ptr> smartFactors;
269 
270  Pose3 lastKeyframePose = Pose3::Identity();
271 
272  // Run one timestep at once:
273  for (const auto &entries : dataset) {
274  // 1) Process new observations:
275  // ------------------------------
276  const auto kf_id = entries.first;
277  const std::vector<stereo_meas_t> &obs = entries.second;
278 
279  // Special instructions for using iSAM2 + smart factors:
280  // Must fill factorNewAffectedKeys:
281  FastMap<FactorIndex, KeySet> factorNewAffectedKeys;
282  NonlinearFactorGraph newFactors;
283 
284  // Map: factor index in the internal graph of iSAM2 => landmark_id
285  std::map<FactorIndex, lm_id_t> newFactor2lm;
286 
287  for (const stereo_meas_t &stObs : obs) {
288  if (smartFactors.count(stObs.lm_id) == 0) {
289  auto noise = noiseModel::Isotropic::Sigma(3, 0.1);
290  SmartProjectionParams params(HESSIAN, ZERO_ON_DEGENERACY);
291 
292  smartFactors[stObs.lm_id] =
293  std::make_shared<SmartStereoProjectionPoseFactor>(noise, params);
294  newFactor2lm[newFactors.size()] = stObs.lm_id;
295  newFactors.push_back(smartFactors[stObs.lm_id]);
296  } else {
297  // Only if the factor *already* existed:
298  factorNewAffectedKeys[lm2factor.at(stObs.lm_id)].insert(X(kf_id));
299  }
300 
301  TEST_COUT("Adding stereo observation from KF #" << kf_id << " for LM #"
302  << stObs.lm_id << "\n");
303 
304  smartFactors[stObs.lm_id]->add(
305  StereoPoint2(stObs.left_u, stObs.right_u, stObs.v), X(kf_id), K);
306  }
307 
308  // prior, for the first keyframe:
309  if (kf_id == 0) {
310  newFactors.addPrior(X(kf_id), Pose3::Identity(), priorPoseNoise);
311  }
312 
313  // 2) Run iSAM2:
314  // ------------------------------
315  Values newValues;
316  newValues.insert(X(kf_id), lastKeyframePose);
317 
318  TEST_COUT("Running iSAM2 for frame: " << kf_id << "\n");
319 
320  ISAM2UpdateParams updateParams;
321  updateParams.newAffectedKeys = std::move(factorNewAffectedKeys);
322 
323  ISAM2Result res = isam.update(newFactors, newValues, updateParams);
324 
325  for (const auto &f2l : newFactor2lm)
326  lm2factor[f2l.second] = res.newFactorsIndices.at(f2l.first);
327 
328  TEST_COUT("error before1: " << res.errorBefore.value() << "\n");
329  TEST_COUT("error after1 : " << res.errorAfter.value() << "\n");
330 
331  // Additional refining steps:
332  ISAM2Result res2 = isam.update();
333  TEST_COUT("error before2: " << res2.errorBefore.value() << "\n");
334  TEST_COUT("error after2 : " << res2.errorAfter.value() << "\n");
335 
336  Values currentEstimate = isam.calculateEstimate();
337 #if TEST_VERBOSE_OUTPUT
338  currentEstimate.print("currentEstimate:");
339 #endif
340 
341  // Keep last KF pose as initial pose of the next one, to reduce the need
342  // to run more non-linear iterations:
343  lastKeyframePose = currentEstimate.at(X(kf_id)).cast<Pose3>();
344 
345  } // end for each timestep
346 
347  Values finalEstimate = isam.calculateEstimate();
348 
349  // GT:
350  // camera +x = vehicle -y
351  // camera +y = vehicle -z
352  // camera +z = vehicle +x
353  for (const auto &gt : gt_positions) {
354  const Pose3 p = finalEstimate.at<Pose3>(X(gt.first));
355  EXPECT(assert_equal(p.x(), -gt.second.y(), tol));
356  EXPECT(assert_equal(p.y(), -gt.second.z(), tol));
357  EXPECT(assert_equal(p.z(), gt.second.x(), tol));
358  }
359 }
360 
361 /* *************************************************************************
362  */
363 int main() {
364  TestResult tr;
365  return TestRegistry::runAllTests(tr);
366 }
TestRegistry::runAllTests
static int runAllTests(TestResult &result)
Definition: TestRegistry.cpp:27
gtsam.examples.DogLegOptimizerExample.int
int
Definition: DogLegOptimizerExample.py:111
stereo_meas_t::v
double v
Definition: testSmartStereoFactor_iSAM2.cpp:61
SmartStereoProjectionPoseFactor.h
Smart stereo factor on poses, assuming camera calibration is fixed.
gtsam::ISAM2Params
Definition: ISAM2Params.h:136
gtsam::NonlinearOptimizer::optimize
virtual const Values & optimize()
Definition: NonlinearOptimizer.h:98
cy
const double cy
Definition: testSmartStereoFactor_iSAM2.cpp:50
gtsam::ISAM2
Definition: ISAM2.h:45
SETDEBUG
#define SETDEBUG(S, V)
Definition: debug.h:61
e
Array< double, 1, 3 > e(1./3., 0.5, 2.)
EXPECT
#define EXPECT(condition)
Definition: Test.h:150
gtsam::HESSIAN
@ HESSIAN
Definition: SmartFactorParams.h:31
TestHarness.h
fx
const double fx
Definition: testSmartStereoFactor_iSAM2.cpp:47
gtsam::SmartProjectionParams
Definition: SmartFactorParams.h:42
LevenbergMarquardtOptimizer.h
A nonlinear optimizer that uses the Levenberg-Marquardt trust-region scheme.
gtsam::FastMap
Definition: FastMap.h:39
gtsam::noiseModel::Diagonal::Sigmas
static shared_ptr Sigmas(const Vector &sigmas, bool smart=true)
Definition: NoiseModel.cpp:291
gtsam::IterativeOptimizationParameters::verbosity
Verbosity verbosity() const
Definition: IterativeSolver.h:61
X
#define X
Definition: icosphere.cpp:20
res
cout<< "Here is the matrix m:"<< endl<< m<< endl;Matrix< ptrdiff_t, 3, 1 > res
Definition: PartialRedux_count.cpp:3
vanilla::params
static const SmartProjectionParams params
Definition: smartFactorScenarios.h:69
gtsam::Vector
Eigen::VectorXd Vector
Definition: Vector.h:38
gtsam::LevenbergMarquardtParams::TRYDELTA
@ TRYDELTA
Definition: LevenbergMarquardtParams.h:40
gtsam::Pose3::Identity
static Pose3 Identity()
identity for group operation
Definition: Pose3.h:106
gtsam::NonlinearISAM::update
void update(const NonlinearFactorGraph &newFactors, const Values &initialValues)
Definition: NonlinearISAM.cpp:35
stereo_meas_t::right_u
double right_u
Definition: testSmartStereoFactor_iSAM2.cpp:61
TEST_COUT
#define TEST_COUT(ARGS_)
Definition: testSmartStereoFactor_iSAM2.cpp:38
TEST
TEST(testISAM2SmartFactor, Stereo_Batch)
Definition: testSmartStereoFactor_iSAM2.cpp:151
gtsam::ZERO_ON_DEGENERACY
@ ZERO_ON_DEGENERACY
Definition: SmartFactorParams.h:36
gtsam::symbol_shorthand::X
Key X(std::uint64_t j)
Definition: inference/Symbol.h:171
gtsam::Values::at
const ValueType at(Key j) const
Definition: Values-inl.h:261
gtsam::Pose3
Definition: Pose3.h:37
parameters
static ConjugateGradientParameters parameters
Definition: testIterative.cpp:33
gtsam::ISAM2Result::errorBefore
std::optional< double > errorBefore
Definition: ISAM2Result.h:52
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
gtsam::LevenbergMarquardtOptimizer
Definition: LevenbergMarquardtOptimizer.h:35
lm_id_t
int lm_id_t
Definition: testSmartStereoFactor_iSAM2.cpp:54
timestep_t
std::size_t timestep_t
Definition: testSmartStereoFactor_iSAM2.cpp:53
size_t
std::size_t size_t
Definition: wrap/pybind11/include/pybind11/detail/common.h:490
TestResult
Definition: TestResult.h:26
stereo_meas_t::left_u
double left_u
Definition: testSmartStereoFactor_iSAM2.cpp:61
stereo_meas_t::lm_id
lm_id_t lm_id
Definition: testSmartStereoFactor_iSAM2.cpp:60
gtsam::FactorGraph::size
size_t size() const
Definition: FactorGraph.h:297
ISAM2.h
Incremental update functionality (ISAM2) for BayesTree, with fluid relinearization.
lu
cout<< "Here is the matrix m:"<< endl<< m<< endl;Eigen::FullPivLU< Matrix5x3 > lu(m)
main
int main()
Definition: testSmartStereoFactor_iSAM2.cpp:363
gtsam::NonlinearOptimizerParams::LINEAR
@ LINEAR
Definition: NonlinearOptimizerParams.h:39
gtsam::Values::print
void print(const std::string &str="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const
Definition: Values.cpp:66
gtsam
traits
Definition: SFMdata.h:40
tol
static const double tol
Definition: testSmartStereoFactor_iSAM2.cpp:42
K
#define K
Definition: igam.h:8
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::StereoPoint2
Definition: StereoPoint2.h:34
gtsam::ISAM2UpdateParams
Definition: ISAM2UpdateParams.h:32
p
float * p
Definition: Tutorial_Map_using.cpp:9
gtsam::Values::insert
void insert(Key j, const Value &val)
Definition: Values.cpp:155
stereo_meas_t
Definition: testSmartStereoFactor_iSAM2.cpp:56
stereo_meas_t::stereo_meas_t
stereo_meas_t(lm_id_t id, double lu, double ru, double v_lr)
Definition: testSmartStereoFactor_iSAM2.cpp:57
gtsam::assert_equal
bool assert_equal(const Matrix &expected, const Matrix &actual, double tol)
Definition: Matrix.cpp:40
baseline
const double baseline
Definition: testSmartStereoFactor_iSAM2.cpp:51
gtsam::LevenbergMarquardtParams
Definition: LevenbergMarquardtParams.h:35
gtsam::tol
const G double tol
Definition: Group.h:79
dataset
static std::map< timestep_t, std::vector< stereo_meas_t > > dataset
Definition: testSmartStereoFactor_iSAM2.cpp:64
gtsam::noiseModel::Isotropic::Sigma
static shared_ptr Sigma(size_t dim, double sigma, bool smart=true)
Definition: NoiseModel.cpp:624
gtsam::ISAM2Result
Definition: ISAM2Result.h:39
gtsam::ISAM2Result::errorAfter
std::optional< double > errorAfter
Definition: ISAM2Result.h:64
cx
const double cx
Definition: testSmartStereoFactor_iSAM2.cpp:49
gtsam::ISAM2UpdateParams::newAffectedKeys
std::optional< FastMap< FactorIndex, KeySet > > newAffectedKeys
Definition: ISAM2UpdateParams.h:66
gtsam::symbol_shorthand::V
Key V(std::uint64_t j)
Definition: inference/Symbol.h:169
gt_positions
static const std::map< timestep_t, gtsam::Point3 > gt_positions
Definition: testSmartStereoFactor_iSAM2.cpp:143
debug.h
Global debugging flags.
fy
const double fy
Definition: testSmartStereoFactor_iSAM2.cpp:48

gtsam
Author(s):
autogenerated on Fri Nov 1 2024 03:41:41