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 SmartProjectionPoseFactorRollingShutter.h
14 * @brief Smart projection factor on poses modeling rolling shutter effect with
15 * given readout time
16 * @author Luca Carlone
17 */
18
19#pragma once
20
21#include <gtsam/geometry/CameraSet.h>
22#include <gtsam/slam/SmartProjectionFactor.h>
23#include <gtsam_unstable/dllexport.h>
24
25namespace gtsam {
26/**
27 *
28 * @ingroup slam
29 *
30 * If you are using the factor, please cite:
31 * L. Carlone, Z. Kira, C. Beall, V. Indelman, F. Dellaert,
32 * Eliminating conditionally independent sets in factor graphs:
33 * a unifying perspective based on smart factors,
34 * Int. Conf. on Robotics and Automation (ICRA), 2014.
35 */
36
37/**
38 * This factor optimizes two consecutive poses of the body assuming a rolling
39 * shutter model of the camera with given readout time. The factor requires that
40 * values contain (for each pixel observation) two consecutive camera poses from
41 * which the pixel observation pose can be interpolated.
42 * @ingroup slam
43 */
44template <class CAMERA>
45class SmartProjectionPoseFactorRollingShutter
46 : public SmartProjectionFactor<CAMERA> {
47 private:
48 typedef SmartProjectionFactor<CAMERA> Base;
49 typedef SmartProjectionPoseFactorRollingShutter<CAMERA> This;
50 typedef typename CAMERA::CalibrationType CALIBRATION;
51 typedef typename CAMERA::Measurement MEASUREMENT;
52 typedef typename CAMERA::MeasurementVector MEASUREMENTS;
53
54 protected:
55 /// The keys of the pose of the body (with respect to an external world
56 /// frame): two consecutive poses for each observation
57 std::vector<std::pair<Key, Key>> world_P_body_key_pairs_;
58
59 /// interpolation factor (one for each observation) to interpolate between
60 /// pair of consecutive poses
61 std::vector<double> alphas_;
62
63 /// one or more cameras taking observations (fixed poses wrt body + fixed
64 /// intrinsics)
65 std::shared_ptr<typename Base::Cameras> cameraRig_;
66
67 /// vector of camera Ids (one for each observation, in the same order),
68 /// identifying which camera took the measurement
69 FastVector<size_t> cameraIds_;
70
71 public:
72 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
73
74 static const int DimBlock =
75 12; ///< size of the variable stacking 2 poses from which the observation
76 ///< pose is interpolated
77 static const int DimPose = 6; ///< Pose3 dimension
78 static const int ZDim = 2; ///< Measurement dimension (Point2)
79 typedef Eigen::Matrix<double, ZDim, DimBlock>
80 MatrixZD; // F blocks (derivatives wrt block of 2 poses)
81 typedef std::vector<MatrixZD, Eigen::aligned_allocator<MatrixZD>>
82 FBlocks; // vector of F blocks
83
84 typedef CAMERA Camera;
85 typedef CameraSet<CAMERA> Cameras;
86
87 /// shorthand for a smart pointer to a factor
88 typedef std::shared_ptr<This> shared_ptr;
89
90 /// Default constructor, only for serialization
91 SmartProjectionPoseFactorRollingShutter() {}
92
93 /**
94 * Constructor
95 * @param Isotropic measurement noise
96 * @param cameraRig set of cameras (fixed poses wrt body and intrinsics)
97 * taking the measurements
98 * @param params internal parameters of the smart factors
99 */
100 SmartProjectionPoseFactorRollingShutter(
101 const SharedNoiseModel& sharedNoiseModel,
102 const std::shared_ptr<Cameras>& cameraRig,
103 const SmartProjectionParams& params = SmartProjectionParams())
104 : Base(sharedNoiseModel, params), cameraRig_(cameraRig) {
105 // throw exception if configuration is not supported by this factor
106 if (Base::params_.degeneracyMode != gtsam::ZERO_ON_DEGENERACY)
107 throw std::runtime_error(
108 "SmartProjectionRigFactor: "
109 "degeneracyMode must be set to ZERO_ON_DEGENERACY");
110 if (Base::params_.linearizationMode != gtsam::HESSIAN)
111 throw std::runtime_error(
112 "SmartProjectionRigFactor: "
113 "linearizationMode must be set to HESSIAN");
114 }
115
116 /**
117 * add a new measurement, with 2 pose keys, interpolation factor, and cameraId
118 * @param measured 2-dimensional location of the projection of a single
119 * landmark in a single view (the measurement), interpolated from the 2 poses
120 * @param world_P_body_key1 key corresponding to the first body poses (time <=
121 * time pixel is acquired)
122 * @param world_P_body_key2 key corresponding to the second body poses (time
123 * >= time pixel is acquired)
124 * @param alpha interpolation factor in [0,1], such that if alpha = 0 the
125 * interpolated pose is the same as world_P_body_key1
126 * @param cameraId ID of the camera taking the measurement (default 0)
127 */
128 void add(const MEASUREMENT& measured, const Key& world_P_body_key1,
129 const Key& world_P_body_key2, const double& alpha,
130 const size_t& cameraId = 0) {
131 // store measurements in base class
132 this->measured_.push_back(measured);
133
134 // store the pair of keys for each measurement, in the same order
135 world_P_body_key_pairs_.push_back(
136 x: std::make_pair(x: world_P_body_key1, y: world_P_body_key2));
137
138 // also store keys in the keys_ vector: these keys are assumed to be
139 // unique, so we avoid duplicates here
140 if (std::find(this->keys_.begin(), this->keys_.end(), world_P_body_key1) ==
141 this->keys_.end())
142 this->keys_.push_back(world_P_body_key1); // add only unique keys
143 if (std::find(this->keys_.begin(), this->keys_.end(), world_P_body_key2) ==
144 this->keys_.end())
145 this->keys_.push_back(world_P_body_key2); // add only unique keys
146
147 // store interpolation factor
148 alphas_.push_back(x: alpha);
149
150 // store id of the camera taking the measurement
151 cameraIds_.push_back(x: cameraId);
152 }
153
154 /**
155 * Variant of the previous "add" function in which we include multiple
156 * measurements
157 * @param measurements vector of the 2m dimensional location of the projection
158 * of a single landmark in the m views (the measurements)
159 * @param world_P_body_key_pairs vector where the i-th element contains a pair
160 * of keys corresponding to the pair of poses from which the observation pose
161 * for the i0-th measurement can be interpolated
162 * @param alphas vector of interpolation params (in [0,1]), one for each
163 * measurement (in the same order)
164 * @param cameraIds IDs of the cameras taking each measurement (same order as
165 * the measurements)
166 */
167 void add(const MEASUREMENTS& measurements,
168 const std::vector<std::pair<Key, Key>>& world_P_body_key_pairs,
169 const std::vector<double>& alphas,
170 const FastVector<size_t>& cameraIds = FastVector<size_t>()) {
171 if (world_P_body_key_pairs.size() != measurements.size() ||
172 world_P_body_key_pairs.size() != alphas.size() ||
173 (world_P_body_key_pairs.size() != cameraIds.size() &&
174 cameraIds.size() != 0)) { // cameraIds.size()=0 is default
175 throw std::runtime_error(
176 "SmartProjectionPoseFactorRollingShutter: "
177 "trying to add inconsistent inputs");
178 }
179 if (cameraIds.size() == 0 && cameraRig_->size() > 1) {
180 throw std::runtime_error(
181 "SmartProjectionPoseFactorRollingShutter: "
182 "camera rig includes multiple camera "
183 "but add did not input cameraIds");
184 }
185 for (size_t i = 0; i < measurements.size(); i++) {
186 add(measurements[i], world_P_body_key_pairs[i].first,
187 world_P_body_key_pairs[i].second, alphas[i],
188 cameraIds.size() == 0 ? 0
189 : cameraIds[i]); // use 0 as default if
190 // cameraIds was not specified
191 }
192 }
193
194 /// return (for each observation) the keys of the pair of poses from which we
195 /// interpolate
196 const std::vector<std::pair<Key, Key>>& world_P_body_key_pairs() const {
197 return world_P_body_key_pairs_;
198 }
199
200 /// return the interpolation factors alphas
201 const std::vector<double>& alphas() const { return alphas_; }
202
203 /// return the calibration object
204 const std::shared_ptr<Cameras>& cameraRig() const { return cameraRig_; }
205
206 /// return the calibration object
207 const FastVector<size_t>& cameraIds() const { return cameraIds_; }
208
209 /**
210 * print
211 * @param s optional string naming the factor
212 * @param keyFormatter optional formatter useful for printing Symbols
213 */
214 void print(
215 const std::string& s = "",
216 const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override {
217 std::cout << s << "SmartProjectionPoseFactorRollingShutter: \n ";
218 for (size_t i = 0; i < cameraIds_.size(); i++) {
219 std::cout << "-- Measurement nr " << i << std::endl;
220 std::cout << " pose1 key: "
221 << keyFormatter(world_P_body_key_pairs_[i].first) << std::endl;
222 std::cout << " pose2 key: "
223 << keyFormatter(world_P_body_key_pairs_[i].second) << std::endl;
224 std::cout << " alpha: " << alphas_[i] << std::endl;
225 std::cout << "cameraId: " << cameraIds_[i] << std::endl;
226 (*cameraRig_)[cameraIds_[i]].print("camera in rig:\n");
227 }
228 Base::print("", keyFormatter);
229 }
230
231 /// equals
232 bool equals(const NonlinearFactor& p, double tol = 1e-9) const override {
233 const SmartProjectionPoseFactorRollingShutter<CAMERA>* e =
234 dynamic_cast<const SmartProjectionPoseFactorRollingShutter<CAMERA>*>(
235 &p);
236
237 double keyPairsEqual = true;
238 if (this->world_P_body_key_pairs_.size() ==
239 e->world_P_body_key_pairs().size()) {
240 for (size_t k = 0; k < this->world_P_body_key_pairs_.size(); k++) {
241 const Key key1own = world_P_body_key_pairs_[k].first;
242 const Key key1e = e->world_P_body_key_pairs()[k].first;
243 const Key key2own = world_P_body_key_pairs_[k].second;
244 const Key key2e = e->world_P_body_key_pairs()[k].second;
245 if (!(key1own == key1e) || !(key2own == key2e)) {
246 keyPairsEqual = false;
247 break;
248 }
249 }
250 } else {
251 keyPairsEqual = false;
252 }
253
254 return e && Base::equals(p, tol) && alphas_ == e->alphas() &&
255 keyPairsEqual && cameraRig_->equals(*(e->cameraRig())) &&
256 std::equal(cameraIds_.begin(), cameraIds_.end(),
257 e->cameraIds().begin());
258 }
259
260 /**
261 * Collect all cameras involved in this factor
262 * @param values Values structure which must contain camera poses
263 * corresponding to keys involved in this factor
264 * @return Cameras
265 */
266 typename Base::Cameras cameras(const Values& values) const override {
267 typename Base::Cameras cameras;
268 for (size_t i = 0; i < this->measured_.size();
269 i++) { // for each measurement
270 const Pose3& w_P_body1 =
271 values.at<Pose3>(j: world_P_body_key_pairs_[i].first);
272 const Pose3& w_P_body2 =
273 values.at<Pose3>(j: world_P_body_key_pairs_[i].second);
274 double interpolationFactor = alphas_[i];
275 const Pose3& w_P_body =
276 interpolate<Pose3>(X: w_P_body1, Y: w_P_body2, t: interpolationFactor);
277 const typename Base::Camera& camera_i = (*cameraRig_)[cameraIds_[i]];
278 const Pose3& body_P_cam = camera_i.pose();
279 const Pose3& w_P_cam = w_P_body.compose(g: body_P_cam);
280 cameras.emplace_back(w_P_cam,
281 make_shared<typename CAMERA::CalibrationType>(
282 camera_i.calibration()));
283 }
284 return cameras;
285 }
286
287 /**
288 * error calculates the error of the factor.
289 */
290 double error(const Values& values) const override {
291 if (this->active(values)) {
292 return this->totalReprojectionError(this->cameras(values));
293 } else { // else of active flag
294 return 0.0;
295 }
296 }
297
298 /**
299 * Compute jacobian F, E and error vector at a given linearization point
300 * @param values Values structure which must contain camera poses
301 * corresponding to keys involved in this factor
302 * @return Return arguments are the camera jacobians Fs (including the
303 * jacobian with respect to both body poses we interpolate from), the point
304 * Jacobian E, and the error vector b. Note that the jacobians are computed
305 * for a given point.
306 */
307 void computeJacobiansWithTriangulatedPoint(FBlocks& Fs, Matrix& E, Vector& b,
308 const Values& values) const {
309 if (!this->result_) {
310 throw("computeJacobiansWithTriangulatedPoint");
311 } else { // valid result: compute jacobians
312 size_t numViews = this->measured_.size();
313 E = Matrix::Zero(rows: 2 * numViews,
314 cols: 3); // a Point2 for each view (point jacobian)
315 b = Vector::Zero(size: 2 * numViews); // a Point2 for each view
316 // intermediate Jacobians
317 Eigen::Matrix<double, ZDim, DimPose> dProject_dPoseCam;
318 Eigen::Matrix<double, DimPose, DimPose> dInterpPose_dPoseBody1,
319 dInterpPose_dPoseBody2, dPoseCam_dInterpPose;
320 Eigen::Matrix<double, ZDim, 3> Ei;
321
322 for (size_t i = 0; i < numViews; i++) { // for each camera/measurement
323 auto w_P_body1 = values.at<Pose3>(j: world_P_body_key_pairs_[i].first);
324 auto w_P_body2 = values.at<Pose3>(j: world_P_body_key_pairs_[i].second);
325 double interpolationFactor = alphas_[i];
326 // get interpolated pose:
327 auto w_P_body =
328 interpolate<Pose3>(X: w_P_body1, Y: w_P_body2, t: interpolationFactor,
329 Hx: dInterpPose_dPoseBody1, Hy: dInterpPose_dPoseBody2);
330 const typename Base::Camera& camera_i = (*cameraRig_)[cameraIds_[i]];
331 auto body_P_cam = camera_i.pose();
332 auto w_P_cam = w_P_body.compose(body_P_cam, dPoseCam_dInterpPose);
333 typename Base::Camera camera(
334 w_P_cam, make_shared<typename CAMERA::CalibrationType>(
335 camera_i.calibration()));
336
337 // get jacobians and error vector for current measurement
338 Point2 reprojectionError_i = camera.reprojectionError(
339 *this->result_, this->measured_.at(i), dProject_dPoseCam, Ei);
340 Eigen::Matrix<double, ZDim, DimBlock> J; // 2 x 12
341 J.block(startRow: 0, startCol: 0, blockRows: ZDim, blockCols: 6) =
342 dProject_dPoseCam * dPoseCam_dInterpPose *
343 dInterpPose_dPoseBody1; // (2x6) * (6x6) * (6x6)
344 J.block(startRow: 0, startCol: 6, blockRows: ZDim, blockCols: 6) =
345 dProject_dPoseCam * dPoseCam_dInterpPose *
346 dInterpPose_dPoseBody2; // (2x6) * (6x6) * (6x6)
347
348 // fit into the output structures
349 Fs.push_back(x: J);
350 size_t row = 2 * i;
351 b.segment<ZDim>(start: row) = -reprojectionError_i;
352 E.block<ZDim, 3>(startRow: row, startCol: 0) = Ei;
353 }
354 }
355 }
356
357 /// linearize and return a Hessianfactor that is an approximation of error(p)
358 std::shared_ptr<RegularHessianFactor<DimPose>> createHessianFactor(
359 const Values& values, const double& lambda = 0.0,
360 bool diagonalDamping = false) const {
361 // we may have multiple observation sharing the same keys (due to the
362 // rolling shutter interpolation), hence the number of unique keys may be
363 // smaller than 2 * nrMeasurements
364 size_t nrUniqueKeys =
365 this->keys_
366 .size(); // note: by construction, keys_ only contains unique keys
367
368 typename Base::Cameras cameras = this->cameras(values);
369
370 // Create structures for Hessian Factors
371 KeyVector js;
372 std::vector<Matrix> Gs(nrUniqueKeys * (nrUniqueKeys + 1) / 2);
373 std::vector<Vector> gs(nrUniqueKeys);
374
375 if (this->measured_.size() !=
376 cameras.size()) // 1 observation per interpolated camera
377 throw std::runtime_error(
378 "SmartProjectionPoseFactorRollingShutter: "
379 "measured_.size() inconsistent with input");
380
381 // triangulate 3D point at given linearization point
382 this->triangulateSafe(cameras);
383
384 if (!this->result_) { // failed: return "empty/zero" Hessian
385 if (this->params_.degeneracyMode == ZERO_ON_DEGENERACY) {
386 for (Matrix& m : Gs) m = Matrix::Zero(rows: DimPose, cols: DimPose);
387 for (Vector& v : gs) v = Vector::Zero(size: DimPose);
388 return std::make_shared<RegularHessianFactor<DimPose>>(this->keys_,
389 Gs, gs, 0.0);
390 } else {
391 throw std::runtime_error(
392 "SmartProjectionPoseFactorRollingShutter: "
393 "only supported degeneracy mode is ZERO_ON_DEGENERACY");
394 }
395 }
396 // compute Jacobian given triangulated 3D Point
397 FBlocks Fs;
398 Matrix E;
399 Vector b;
400 this->computeJacobiansWithTriangulatedPoint(Fs, E, b, values);
401
402 // Whiten using noise model
403 this->noiseModel_->WhitenSystem(E, b);
404 for (size_t i = 0; i < Fs.size(); i++)
405 Fs[i] = this->noiseModel_->Whiten(Fs[i]);
406
407 Matrix3 P = Cameras::PointCov(E, lambda, diagonalDamping);
408
409 // Collect all the key pairs: these are the keys that correspond to the
410 // blocks in Fs (on which we apply the Schur Complement)
411 KeyVector nonuniqueKeys;
412 for (size_t i = 0; i < world_P_body_key_pairs_.size(); i++) {
413 nonuniqueKeys.push_back(x: world_P_body_key_pairs_.at(n: i).first);
414 nonuniqueKeys.push_back(x: world_P_body_key_pairs_.at(n: i).second);
415 }
416
417 // Build augmented Hessian (with last row/column being the information
418 // vector) Note: we need to get the augumented hessian wrt the unique keys
419 // in key_
420 SymmetricBlockMatrix augmentedHessianUniqueKeys =
421 Base::Cameras::template SchurComplementAndRearrangeBlocks<3, 12, 6>(
422 Fs, E, P, b, nonuniqueKeys, this->keys_);
423
424 return std::make_shared<RegularHessianFactor<DimPose>>(
425 this->keys_, augmentedHessianUniqueKeys);
426 }
427
428 /**
429 * Linearize to Gaussian Factor (possibly adding a damping factor Lambda for
430 * LM)
431 * @param values Values structure which must contain camera poses and
432 * extrinsic pose for this factor
433 * @return a Gaussian factor
434 */
435 std::shared_ptr<GaussianFactor> linearizeDamped(
436 const Values& values, const double& lambda = 0.0) const {
437 // depending on flag set on construction we may linearize to different
438 // linear factors
439 switch (this->params_.linearizationMode) {
440 case HESSIAN:
441 return this->createHessianFactor(values, lambda);
442 default:
443 throw std::runtime_error(
444 "SmartProjectionPoseFactorRollingShutter: "
445 "unknown linearization mode");
446 }
447 }
448
449 /// linearize
450 std::shared_ptr<GaussianFactor> linearize(
451 const Values& values) const override {
452 return this->linearizeDamped(values);
453 }
454
455 private:
456#if GTSAM_ENABLE_BOOST_SERIALIZATION ///
457 /// Serialization function
458 friend class boost::serialization::access;
459 template <class ARCHIVE>
460 void serialize(ARCHIVE& ar, const unsigned int /*version*/) {
461 ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base);
462 }
463#endif
464};
465// end of class declaration
466
467/// traits
468template <class CAMERA>
469struct traits<SmartProjectionPoseFactorRollingShutter<CAMERA>>
470 : public Testable<SmartProjectionPoseFactorRollingShutter<CAMERA>> {};
471
472} // namespace gtsam
473