1/* ----------------------------------------------------------------------------
2
3 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
4 * Atlanta, Georgia 30332-0415
5 * All Rights Reserved
6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
7
8 * See LICENSE for the license information
9
10 * -------------------------------------------------------------------------- */
11
12/**
13 * @file SmartFactorParams.h
14 * @brief Collect common parameters for SmartProjection and SmartStereoProjection factors
15 * @author Luca Carlone
16 * @author Zsolt Kira
17 * @author Frank Dellaert
18 */
19
20#pragma once
21
22#include <gtsam/geometry/triangulation.h>
23
24namespace gtsam {
25
26/**
27 * SmartFactorParams: parameters and (linearization/degeneracy) modes for SmartProjection and SmartStereoProjection factors
28 */
29/// Linearization mode: what factor to linearize to
30enum LinearizationMode {
31 HESSIAN, IMPLICIT_SCHUR, JACOBIAN_Q, JACOBIAN_SVD
32};
33
34/// How to manage degeneracy
35enum DegeneracyMode {
36 IGNORE_DEGENERACY, ZERO_ON_DEGENERACY, HANDLE_INFINITY
37};
38
39/*
40 * Parameters for the smart (stereo) projection factors
41 */
42struct SmartProjectionParams {
43
44 LinearizationMode linearizationMode; ///< How to linearize the factor
45 DegeneracyMode degeneracyMode; ///< How to linearize the factor
46
47 /// @name Parameters governing the triangulation
48 /// @{
49 TriangulationParameters triangulation;
50 double retriangulationThreshold; ///< threshold to decide whether to re-triangulate
51 /// @}
52
53 /// @name Parameters governing how triangulation result is treated
54 /// @{
55 bool throwCheirality; ///< If true, re-throws Cheirality exceptions (default: false)
56 bool verboseCheirality; ///< If true, prints text for Cheirality exceptions (default: false)
57 /// @}
58
59 // Constructor
60 SmartProjectionParams(LinearizationMode linMode = HESSIAN,
61 DegeneracyMode degMode = IGNORE_DEGENERACY, bool throwCheirality = false,
62 bool verboseCheirality = false, double retriangulationTh = 1e-5) :
63 linearizationMode(linMode), degeneracyMode(degMode), retriangulationThreshold(
64 retriangulationTh), throwCheirality(throwCheirality), verboseCheirality(
65 verboseCheirality) {
66 }
67
68 virtual ~SmartProjectionParams() {
69 }
70
71 void print(const std::string& str = "") const {
72 std::cout << "linearizationMode: " << linearizationMode << "\n";
73 std::cout << " degeneracyMode: " << degeneracyMode << "\n";
74 std::cout << triangulation << std::endl;
75 }
76
77 // get class variables
78 LinearizationMode getLinearizationMode() const {
79 return linearizationMode;
80 }
81 DegeneracyMode getDegeneracyMode() const {
82 return degeneracyMode;
83 }
84 TriangulationParameters getTriangulationParameters() const {
85 return triangulation;
86 }
87 bool getVerboseCheirality() const {
88 return verboseCheirality;
89 }
90 bool getThrowCheirality() const {
91 return throwCheirality;
92 }
93 double getRetriangulationThreshold() const {
94 return retriangulationThreshold;
95 }
96 // set class variables
97 void setLinearizationMode(LinearizationMode linMode) {
98 linearizationMode = linMode;
99 }
100 void setDegeneracyMode(DegeneracyMode degMode) {
101 degeneracyMode = degMode;
102 }
103 void setRetriangulationThreshold(double retriangulationTh) {
104 retriangulationThreshold = retriangulationTh;
105 }
106 void setRankTolerance(double rankTol) {
107 triangulation.rankTolerance = rankTol;
108 }
109 void setEnableEPI(bool enableEPI) {
110 triangulation.enableEPI = enableEPI;
111 }
112 void setLandmarkDistanceThreshold(double landmarkDistanceThreshold) {
113 triangulation.landmarkDistanceThreshold = landmarkDistanceThreshold;
114 }
115 void setDynamicOutlierRejectionThreshold(double dynOutRejectionThreshold) {
116 triangulation.dynamicOutlierRejectionThreshold = dynOutRejectionThreshold;
117 }
118
119private:
120
121#if GTSAM_ENABLE_BOOST_SERIALIZATION ///
122 /// Serialization function
123 friend class boost::serialization::access;
124 template<class ARCHIVE>
125 void serialize(ARCHIVE & ar, const unsigned int version) {
126 ar & BOOST_SERIALIZATION_NVP(linearizationMode);
127 ar & BOOST_SERIALIZATION_NVP(degeneracyMode);
128 ar & BOOST_SERIALIZATION_NVP(triangulation);
129 ar & BOOST_SERIALIZATION_NVP(retriangulationThreshold);
130 ar & BOOST_SERIALIZATION_NVP(throwCheirality);
131 ar & BOOST_SERIALIZATION_NVP(verboseCheirality);
132 }
133#endif
134};
135
136} // \ namespace gtsam
137