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 SmartStereoProjectionFactorPP.h
14 * @brief Smart stereo factor on poses (P) and camera extrinsic pose (P) calibrations
15 * @author Luca Carlone
16 * @author Frank Dellaert
17 */
18
19#pragma once
20
21#include <gtsam_unstable/slam/SmartStereoProjectionFactor.h>
22
23namespace gtsam {
24/**
25 *
26 * @ingroup slam
27 *
28 * If you are using the factor, please cite:
29 * L. Carlone, Z. Kira, C. Beall, V. Indelman, F. Dellaert,
30 * Eliminating conditionally independent sets in factor graphs:
31 * a unifying perspective based on smart factors,
32 * Int. Conf. on Robotics and Automation (ICRA), 2014.
33 */
34
35/**
36 * This factor optimizes the pose of the body as well as the extrinsic camera
37 * calibration (pose of camera wrt body). Each camera may have its own extrinsic
38 * calibration or the same calibration can be shared by multiple cameras. This
39 * factor requires that values contain the involved poses and extrinsics (both
40 * are Pose3 variables).
41 * @ingroup slam
42 */
43class GTSAM_UNSTABLE_EXPORT SmartStereoProjectionFactorPP
44 : public SmartStereoProjectionFactor {
45 protected:
46 /// shared pointer to calibration object (one for each camera)
47 std::vector<std::shared_ptr<Cal3_S2Stereo>> K_all_;
48
49 /// The keys corresponding to the pose of the body (with respect to an external world frame) for each view
50 KeyVector world_P_body_keys_;
51
52 /// The keys corresponding to the extrinsic pose calibration for each view (pose that transform from camera to body)
53 KeyVector body_P_cam_keys_;
54
55 public:
56 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
57
58 /// shorthand for base class type
59 typedef SmartStereoProjectionFactor Base;
60
61 /// shorthand for this class
62 typedef SmartStereoProjectionFactorPP This;
63
64 /// shorthand for a smart pointer to a factor
65 typedef std::shared_ptr<This> shared_ptr;
66
67 static const int DimBlock = 12; ///< Camera dimension: 6 for body pose, 6 for extrinsic pose
68 static const int DimPose = 6; ///< Pose3 dimension
69 static const int ZDim = 3; ///< Measurement dimension (for a StereoPoint2 measurement)
70 typedef Eigen::Matrix<double, ZDim, DimBlock> MatrixZD; // F blocks (derivatives wrt camera)
71 typedef std::vector<MatrixZD, Eigen::aligned_allocator<MatrixZD> > FBlocks; // vector of F blocks
72
73 /**
74 * Constructor
75 * @param Isotropic measurement noise
76 * @param params internal parameters of the smart factors
77 */
78 SmartStereoProjectionFactorPP(const SharedNoiseModel& sharedNoiseModel,
79 const SmartStereoProjectionParams& params =
80 SmartStereoProjectionParams());
81
82 /**
83 * add a new measurement, with a pose key, and an extrinsic pose key
84 * @param measured is the 3-dimensional location of the projection of a
85 * single landmark in the a single (stereo) view (the measurement)
86 * @param world_P_body_key is the key corresponding to the body poses observing the same landmark
87 * @param body_P_cam_key is the key corresponding to the extrinsic camera-to-body pose calibration
88 * @param K is the (fixed) camera intrinsic calibration
89 */
90 void add(const StereoPoint2& measured, const Key& world_P_body_key,
91 const Key& body_P_cam_key,
92 const std::shared_ptr<Cal3_S2Stereo>& K);
93
94 /**
95 * Variant of the previous one in which we include a set of measurements
96 * @param measurements vector of the 3m dimensional location of the projection
97 * of a single landmark in the m (stereo) view (the measurements)
98 * @param w_P_body_keys are the ordered keys corresponding to the body poses observing the same landmark
99 * @param body_P_cam_keys are the ordered keys corresponding to the extrinsic camera-to-body poses calibration
100 * (note: elements of this vector do not need to be unique: 2 camera views can share the same calibration)
101 * @param Ks vector of intrinsic calibration objects
102 */
103 void add(const std::vector<StereoPoint2>& measurements,
104 const KeyVector& w_P_body_keys, const KeyVector& body_P_cam_keys,
105 const std::vector<std::shared_ptr<Cal3_S2Stereo>>& Ks);
106
107 /**
108 * Variant of the previous one in which we include a set of measurements with
109 * the same noise and calibration
110 * @param measurements vector of the 3m dimensional location of the projection
111 * of a single landmark in the m (stereo) view (the measurements)
112 * @param w_P_body_keys are the ordered keys corresponding to the body poses observing the same landmark
113 * @param body_P_cam_keys are the ordered keys corresponding to the extrinsic camera-to-body poses calibration
114 * (note: elements of this vector do not need to be unique: 2 camera views can share the same calibration)
115 * @param K the (known) camera calibration (same for all measurements)
116 */
117 void add(const std::vector<StereoPoint2>& measurements,
118 const KeyVector& w_P_body_keys, const KeyVector& body_P_cam_keys,
119 const std::shared_ptr<Cal3_S2Stereo>& K);
120
121 /**
122 * print
123 * @param s optional string naming the factor
124 * @param keyFormatter optional formatter useful for printing Symbols
125 */
126 void print(const std::string& s = "", const KeyFormatter& keyFormatter =
127 DefaultKeyFormatter) const override;
128
129 /// equals
130 bool equals(const NonlinearFactor& p, double tol = 1e-9) const override;
131
132 /// equals
133 const KeyVector& getExtrinsicPoseKeys() const {
134 return body_P_cam_keys_;
135 }
136
137 /**
138 * error calculates the error of the factor.
139 */
140 double error(const Values& values) const override;
141
142 /** return the calibration object */
143 inline std::vector<std::shared_ptr<Cal3_S2Stereo>> calibration() const {
144 return K_all_;
145 }
146
147 /**
148 * Collect all cameras involved in this factor
149 * @param values Values structure which must contain camera poses
150 * corresponding
151 * to keys involved in this factor
152 * @return vector of Values
153 */
154 Base::Cameras cameras(const Values& values) const override;
155
156 /**
157 * Compute jacobian F, E and error vector at a given linearization point
158 * @param values Values structure which must contain camera poses
159 * corresponding to keys involved in this factor
160 * @return Return arguments are the camera jacobians Fs (including the jacobian with
161 * respect to both the body pose and extrinsic pose), the point Jacobian E,
162 * and the error vector b. Note that the jacobians are computed for a given point.
163 */
164 void computeJacobiansAndCorrectForMissingMeasurements(
165 FBlocks& Fs, Matrix& E, Vector& b, const Values& values) const {
166 if (!result_) {
167 throw("computeJacobiansWithTriangulatedPoint");
168 } else { // valid result: compute jacobians
169 size_t numViews = measured_.size();
170 E = Matrix::Zero(rows: 3 * numViews, cols: 3); // a StereoPoint2 for each view (point jacobian)
171 b = Vector::Zero(size: 3 * numViews); // a StereoPoint2 for each view
172 Matrix dPoseCam_dPoseBody_i, dPoseCam_dPoseExt_i, dProject_dPoseCam_i, Ei;
173
174 for (size_t i = 0; i < numViews; i++) { // for each camera/measurement
175 Pose3 w_P_body = values.at<Pose3>(j: world_P_body_keys_.at(n: i));
176 Pose3 body_P_cam = values.at<Pose3>(j: body_P_cam_keys_.at(n: i));
177 StereoCamera camera(
178 w_P_body.compose(g: body_P_cam, H1: dPoseCam_dPoseBody_i, H2: dPoseCam_dPoseExt_i),
179 K_all_[i]);
180 // get jacobians and error vector for current measurement
181 StereoPoint2 reprojectionError_i = StereoPoint2(
182 camera.project(point: *result_, H1: dProject_dPoseCam_i, H2: Ei) - measured_.at(n: i));
183 Eigen::Matrix<double, ZDim, DimBlock> J; // 3 x 12
184 J.block<ZDim, 6>(startRow: 0, startCol: 0) = dProject_dPoseCam_i * dPoseCam_dPoseBody_i; // (3x6) * (6x6)
185 J.block<ZDim, 6>(startRow: 0, startCol: 6) = dProject_dPoseCam_i * dPoseCam_dPoseExt_i; // (3x6) * (6x6)
186 // if the right pixel is invalid, fix jacobians
187 if (std::isnan(x: measured_.at(n: i).uR()))
188 {
189 J.block<1, 12>(startRow: 1, startCol: 0) = Matrix::Zero(rows: 1, cols: 12);
190 Ei.block<1, 3>(startRow: 1, startCol: 0) = Matrix::Zero(rows: 1, cols: 3);
191 reprojectionError_i = StereoPoint2(reprojectionError_i.uL(), 0.0,
192 reprojectionError_i.v());
193 }
194 // fit into the output structures
195 Fs.push_back(x: J);
196 size_t row = 3 * i;
197 b.segment<ZDim>(start: row) = -reprojectionError_i.vector();
198 E.block<3, 3>(startRow: row, startCol: 0) = Ei;
199 }
200 }
201 }
202
203 /// linearize and return a Hessianfactor that is an approximation of error(p)
204 std::shared_ptr<RegularHessianFactor<DimPose>> createHessianFactor(
205 const Values& values, const double lambda = 0.0,
206 bool diagonalDamping = false) const {
207 // we may have multiple cameras sharing the same extrinsic cals, hence the number
208 // of keys may be smaller than 2 * nrMeasurements (which is the upper bound where we
209 // have a body key and an extrinsic calibration key for each measurement)
210 size_t nrUniqueKeys = keys_.size();
211
212 // Create structures for Hessian Factors
213 KeyVector js;
214 std::vector<Matrix> Gs(nrUniqueKeys * (nrUniqueKeys + 1) / 2);
215 std::vector<Vector> gs(nrUniqueKeys);
216
217 if (this->measured_.size() != cameras(values).size())
218 throw std::runtime_error("SmartStereoProjectionHessianFactor: this->"
219 "measured_.size() inconsistent with input");
220
221 // triangulate 3D point at given linearization point
222 triangulateSafe(cameras: cameras(values));
223
224 // failed: return "empty/zero" Hessian
225 if (!result_) {
226 for (Matrix& m : Gs) m = Matrix::Zero(rows: DimPose, cols: DimPose);
227 for (Vector& v : gs) v = Vector::Zero(size: DimPose);
228 return std::make_shared<RegularHessianFactor<DimPose>>(args: keys_, args&: Gs, args&: gs,
229 args: 0.0);
230 }
231
232 // compute Jacobian given triangulated 3D Point
233 FBlocks Fs;
234 Matrix F, E;
235 Vector b;
236 computeJacobiansAndCorrectForMissingMeasurements(Fs, E, b, values);
237
238 // Whiten using noise model
239 noiseModel_->WhitenSystem(A&: E, b);
240 for (size_t i = 0; i < Fs.size(); i++) {
241 Fs[i] = noiseModel_->Whiten(H: Fs[i]);
242 }
243
244 // build augmented Hessian (with last row/column being the information vector)
245 Matrix3 P;
246 Cameras::ComputePointCovariance<3>(P, E, lambda, diagonalDamping);
247
248 // these are the keys that correspond to the blocks in augmentedHessian (output of SchurComplement)
249 KeyVector nonuniqueKeys;
250 for (size_t i = 0; i < world_P_body_keys_.size(); i++) {
251 nonuniqueKeys.push_back(x: world_P_body_keys_.at(n: i));
252 nonuniqueKeys.push_back(x: body_P_cam_keys_.at(n: i));
253 }
254 // but we need to get the augumented hessian wrt the unique keys in key_
255 SymmetricBlockMatrix augmentedHessianUniqueKeys =
256 Base::Cameras::template SchurComplementAndRearrangeBlocks<3, DimBlock,
257 DimPose>(
258 Fs, E, P, b, jacobianKeys: nonuniqueKeys, hessianKeys: keys_);
259
260 return std::make_shared<RegularHessianFactor<DimPose>>(
261 args: keys_, args&: augmentedHessianUniqueKeys);
262 }
263
264 /**
265 * Linearize to Gaussian Factor (possibly adding a damping factor Lambda for LM)
266 * @param values Values structure which must contain camera poses and extrinsic pose for this factor
267 * @return a Gaussian factor
268 */
269 std::shared_ptr<GaussianFactor> linearizeDamped(
270 const Values& values, const double lambda = 0.0) const {
271 // depending on flag set on construction we may linearize to different linear factors
272 switch (params_.linearizationMode) {
273 case HESSIAN:
274 return createHessianFactor(values, lambda);
275 default:
276 throw std::runtime_error(
277 "SmartStereoProjectionFactorPP: unknown linearization mode");
278 }
279 }
280
281 /// linearize
282 std::shared_ptr<GaussianFactor> linearize(const Values& values) const
283 override {
284 return linearizeDamped(values);
285 }
286
287 private:
288#if GTSAM_ENABLE_BOOST_SERIALIZATION ///
289 /// Serialization function
290 friend class boost::serialization::access;
291 template<class ARCHIVE>
292 void serialize(ARCHIVE& ar, const unsigned int /*version*/) {
293 ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base);
294 ar & BOOST_SERIALIZATION_NVP(K_all_);
295 }
296#endif
297};
298// end of class declaration
299
300/// traits
301template<>
302struct traits<SmartStereoProjectionFactorPP> : public Testable<
303 SmartStereoProjectionFactorPP> {
304};
305
306} // namespace gtsam
307