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 ProjectionFactorRollingShutter.cpp
14 * @brief Basic projection factor for rolling shutter cameras
15 * @author Yotam Stern
16 */
17
18#include <gtsam_unstable/slam/ProjectionFactorRollingShutter.h>
19
20namespace gtsam {
21
22Vector ProjectionFactorRollingShutter::evaluateError(
23 const Pose3& pose_a, const Pose3& pose_b, const Point3& point,
24 OptionalMatrixType H1, OptionalMatrixType H2,
25 OptionalMatrixType H3) const {
26 try {
27 Pose3 pose = interpolate<Pose3>(X: pose_a, Y: pose_b, t: alpha_, Hx: H1, Hy: H2);
28 gtsam::Matrix Hprj;
29 if (body_P_sensor_) {
30 if (H1 || H2 || H3) {
31 gtsam::Matrix HbodySensor;
32 PinholeCamera<Cal3_S2> camera(
33 pose.compose(g: *body_P_sensor_, H1: HbodySensor), *K_);
34 Point2 reprojectionError(camera.project(pw: point, Dpose: Hprj, Dpoint: H3, Dcal: {}) -
35 measured_);
36 if (H1) *H1 = Hprj * HbodySensor * (*H1);
37 if (H2) *H2 = Hprj * HbodySensor * (*H2);
38 return reprojectionError;
39 } else {
40 PinholeCamera<Cal3_S2> camera(pose.compose(g: *body_P_sensor_), *K_);
41 return camera.project(pw: point) - measured_;
42 }
43 } else {
44 PinholeCamera<Cal3_S2> camera(pose, *K_);
45 Point2 reprojectionError(camera.project(pw: point, Dpose: Hprj, Dpoint: H3, Dcal: {}) -
46 measured_);
47 if (H1) *H1 = Hprj * (*H1);
48 if (H2) *H2 = Hprj * (*H2);
49 return reprojectionError;
50 }
51 } catch (CheiralityException& e) {
52 if (H1) *H1 = Matrix::Zero(rows: 2, cols: 6);
53 if (H2) *H2 = Matrix::Zero(rows: 2, cols: 6);
54 if (H3) *H3 = Matrix::Zero(rows: 2, cols: 3);
55 if (verboseCheirality_)
56 std::cout << e.what() << ": Landmark "
57 << DefaultKeyFormatter(this->key<2>()) << " moved behind camera "
58 << DefaultKeyFormatter(this->key<1>()) << std::endl;
59 if (throwCheirality_) throw CheiralityException(this->key<2>());
60 }
61 return Vector2::Constant(value: 2.0 * K_->fx());
62}
63
64} // namespace gtsam
65