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 SmartStereoProjectionPoseFactor.h
14 * @brief Smart stereo factor on poses, assuming camera calibration is fixed
15 * @author Luca Carlone
16 * @author Antoni Rosinol
17 * @author Chris Beall
18 * @author Zsolt Kira
19 * @author Frank Dellaert
20 */
21
22#pragma once
23
24#include <gtsam_unstable/slam/SmartStereoProjectionFactor.h>
25
26namespace gtsam {
27/**
28 *
29 * @ingroup slam
30 *
31 * If you are using the factor, please cite:
32 * L. Carlone, Z. Kira, C. Beall, V. Indelman, F. Dellaert,
33 * Eliminating conditionally independent sets in factor graphs:
34 * a unifying perspective based on smart factors,
35 * Int. Conf. on Robotics and Automation (ICRA), 2014.
36 *
37 */
38
39/**
40 * This factor assumes that camera calibration is fixed, but each camera
41 * has its own calibration.
42 * The factor only constrains poses (variable dimension is 6).
43 * This factor requires that values contains the involved poses (Pose3).
44 * @ingroup slam
45 */
46class GTSAM_UNSTABLE_EXPORT SmartStereoProjectionPoseFactor
47 : public SmartStereoProjectionFactor {
48 protected:
49 /// shared pointer to calibration object (one for each camera)
50 std::vector<std::shared_ptr<Cal3_S2Stereo>> K_all_;
51
52 public:
53 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
54
55 /// shorthand for base class type
56 typedef SmartStereoProjectionFactor Base;
57
58 /// shorthand for this class
59 typedef SmartStereoProjectionPoseFactor This;
60
61 /// shorthand for a smart pointer to a factor
62 typedef std::shared_ptr<This> shared_ptr;
63
64 /**
65 * Constructor
66 * @param Isotropic measurement noise
67 * @param params internal parameters of the smart factors
68 */
69 SmartStereoProjectionPoseFactor(
70 const SharedNoiseModel& sharedNoiseModel,
71 const SmartStereoProjectionParams& params = SmartStereoProjectionParams(),
72 const std::optional<Pose3>& body_P_sensor = {});
73
74 /**
75 * add a new measurement and pose key
76 * @param measured is the 2m dimensional location of the projection of a
77 * single landmark in the m view (the measurement)
78 * @param poseKey is key corresponding to the camera observing the same
79 * landmark
80 * @param K is the (fixed) camera calibration
81 */
82 void add(const StereoPoint2& measured, const Key& poseKey,
83 const std::shared_ptr<Cal3_S2Stereo>& K);
84
85 /**
86 * Variant of the previous one in which we include a set of measurements
87 * @param measurements vector of the 2m dimensional location of the projection
88 * of a single landmark in the m view (the measurement)
89 * @param poseKeys vector of keys corresponding to the camera observing
90 * the same landmark
91 * @param Ks vector of calibration objects
92 */
93 void add(const std::vector<StereoPoint2>& measurements,
94 const KeyVector& poseKeys,
95 const std::vector<std::shared_ptr<Cal3_S2Stereo>>& Ks);
96
97 /**
98 * Variant of the previous one in which we include a set of measurements with
99 * the same noise and calibration
100 * @param measurements vector of the 2m dimensional location of the projection
101 * of a single landmark in the m view (the measurement)
102 * @param poseKeys vector of keys corresponding to the camera observing the
103 * same landmark
104 * @param K the (known) camera calibration (same for all measurements)
105 */
106 void add(const std::vector<StereoPoint2>& measurements,
107 const KeyVector& poseKeys,
108 const std::shared_ptr<Cal3_S2Stereo>& K);
109
110 /**
111 * print
112 * @param s optional string naming the factor
113 * @param keyFormatter optional formatter useful for printing Symbols
114 */
115 void print(const std::string& s = "", const KeyFormatter& keyFormatter =
116 DefaultKeyFormatter) const override;
117
118 /// equals
119 bool equals(const NonlinearFactor& p, double tol = 1e-9) const override;
120
121 /**
122 * error calculates the error of the factor.
123 */
124 double error(const Values& values) const override;
125
126 /** return the calibration object */
127 inline std::vector<std::shared_ptr<Cal3_S2Stereo>> calibration() const {
128 return K_all_;
129 }
130
131 /**
132 * Collect all cameras involved in this factor
133 * @param values Values structure which must contain camera poses
134 * corresponding
135 * to keys involved in this factor
136 * @return vector of Values
137 */
138 Base::Cameras cameras(const Values& values) const override;
139
140 private:
141#if GTSAM_ENABLE_BOOST_SERIALIZATION ///
142 /// Serialization function
143 friend class boost::serialization::access;
144 template <class ARCHIVE>
145 void serialize(ARCHIVE& ar, const unsigned int /*version*/) {
146 ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base);
147 ar& BOOST_SERIALIZATION_NVP(K_all_);
148 }
149#endif
150
151}; // end of class declaration
152
153/// traits
154template <>
155struct traits<SmartStereoProjectionPoseFactor>
156 : public Testable<SmartStereoProjectionPoseFactor> {};
157
158} // namespace gtsam
159