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 ProjectionFactorPPPC.h
14 * @brief Derived from ProjectionFactor, but estimates body-camera transform
15 * and calibration in addition to body pose and 3D landmark
16 * @author Chris Beall
17 */
18
19#pragma once
20
21#include <gtsam/geometry/Cal3_S2.h>
22#include <gtsam/geometry/PinholeCamera.h>
23#include <gtsam/nonlinear/NonlinearFactor.h>
24#include <gtsam_unstable/dllexport.h>
25
26
27namespace gtsam {
28
29 /**
30 * Non-linear factor for a constraint derived from a 2D measurement. This factor
31 * estimates the body pose, body-camera transform, 3D landmark, and calibration.
32 * @ingroup slam
33 */
34template <class POSE, class LANDMARK, class CALIBRATION = Cal3_S2>
35class ProjectionFactorPPPC
36 : public NoiseModelFactorN<POSE, POSE, LANDMARK, CALIBRATION> {
37 protected:
38 Point2 measured_; ///< 2D measurement
39
40 // verbosity handling for Cheirality Exceptions
41 bool throwCheirality_; ///< If true, rethrows Cheirality exceptions (default: false)
42 bool verboseCheirality_; ///< If true, prints text for Cheirality exceptions (default: false)
43
44 public:
45 /// shorthand for base class type
46 typedef NoiseModelFactor4<POSE, POSE, LANDMARK, CALIBRATION> Base;
47
48 // Provide access to the Matrix& version of evaluateError:
49 using Base::evaluateError;
50
51 /// shorthand for this class
52 typedef ProjectionFactorPPPC<POSE, LANDMARK, CALIBRATION> This;
53
54 /// shorthand for a smart pointer to a factor
55 typedef std::shared_ptr<This> shared_ptr;
56
57 /// Default constructor
58 ProjectionFactorPPPC() :
59 measured_(0.0, 0.0), throwCheirality_(false), verboseCheirality_(false) {
60 }
61
62
63 /**
64 * Constructor with exception-handling flags
65 * TODO: Mark argument order standard (keys, measurement, parameters)
66 * @param measured is the 2 dimensional location of point in image (the
67 * measurement)
68 * @param model is the standard deviation
69 * @param poseKey is the index of the camera
70 * @param transformKey is the index of the extrinsic calibration
71 * @param pointKey is the index of the landmark
72 * @param calibKey is the index of the intrinsic calibration
73 * @param throwCheirality determines whether Cheirality exceptions are
74 * rethrown
75 * @param verboseCheirality determines whether exceptions are printed for
76 * Cheirality
77 */
78 ProjectionFactorPPPC(const Point2& measured, const SharedNoiseModel& model,
79 Key poseKey, Key transformKey, Key pointKey, Key calibKey,
80 bool throwCheirality = false, bool verboseCheirality = false) :
81 Base(model, poseKey, transformKey, pointKey, calibKey), measured_(measured),
82 throwCheirality_(throwCheirality), verboseCheirality_(verboseCheirality) {}
83
84 /** Virtual destructor */
85 ~ProjectionFactorPPPC() override {}
86
87 /// @return a deep copy of this factor
88 NonlinearFactor::shared_ptr clone() const override {
89 return std::static_pointer_cast<NonlinearFactor>(
90 r: NonlinearFactor::shared_ptr(new This(*this))); }
91
92 /**
93 * print
94 * @param s optional string naming the factor
95 * @param keyFormatter optional formatter useful for printing Symbols
96 */
97 void print(const std::string& s = "", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override {
98 std::cout << s << "ProjectionFactorPPPC, z = ";
99 traits<Point2>::Print(m: measured_);
100 Base::print("", keyFormatter);
101 }
102
103 /// equals
104 bool equals(const NonlinearFactor& p, double tol = 1e-9) const override {
105 const This *e = dynamic_cast<const This*>(&p);
106 return e
107 && Base::equals(p, tol)
108 && traits<Point2>::Equals(v1: this->measured_, v2: e->measured_, tol);
109 }
110
111 /// Evaluate error h(x)-z and optionally derivatives
112 Vector evaluateError(const Pose3& pose, const Pose3& transform, const Point3& point, const CALIBRATION& K,
113 OptionalMatrixType H1, OptionalMatrixType H2, OptionalMatrixType H3,
114 OptionalMatrixType H4) const override {
115 try {
116 if(H1 || H2 || H3 || H4) {
117 Matrix H0, H02;
118 const PinholeCamera<CALIBRATION> camera(pose.compose(g: transform, H1: H0, H2: H02), K);
119 const Point2 reprojectionError(camera.project(point, H1, H3, H4) - measured_);
120 *H2 = *H1 * H02;
121 *H1 = *H1 * H0;
122 return reprojectionError;
123 } else {
124 PinholeCamera<CALIBRATION> camera(pose.compose(g: transform), K);
125 return camera.project(point, H1, H3, H4) - measured_;
126 }
127 } catch( CheiralityException& e) {
128 if (H1) *H1 = Matrix::Zero(rows: 2,cols: 6);
129 if (H2) *H2 = Matrix::Zero(rows: 2,cols: 6);
130 if (H3) *H3 = Matrix::Zero(rows: 2,cols: 3);
131 if (H4) *H4 = Matrix::Zero(2,CALIBRATION::dimension);
132 if (verboseCheirality_)
133 std::cout << e.what() << ": Landmark "<< DefaultKeyFormatter(this->key2()) <<
134 " moved behind camera " << DefaultKeyFormatter(this->key1()) << std::endl;
135 if (throwCheirality_)
136 throw e;
137 }
138 return Vector::Ones(newSize: 2) * 2.0 * K.fx();
139 }
140
141 /** return the measurement */
142 const Point2& measured() const {
143 return measured_;
144 }
145
146 /** return verbosity */
147 inline bool verboseCheirality() const { return verboseCheirality_; }
148
149 /** return flag for throwing cheirality exceptions */
150 inline bool throwCheirality() const { return throwCheirality_; }
151
152 private:
153
154#if GTSAM_ENABLE_BOOST_SERIALIZATION ///
155 /// Serialization function
156 friend class boost::serialization::access;
157 template<class ARCHIVE>
158 void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
159 ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base);
160 ar & BOOST_SERIALIZATION_NVP(measured_);
161 ar & BOOST_SERIALIZATION_NVP(throwCheirality_);
162 ar & BOOST_SERIALIZATION_NVP(verboseCheirality_);
163 }
164#endif
165};
166
167 /// traits
168 template<class POSE, class LANDMARK, class CALIBRATION>
169 struct traits<ProjectionFactorPPPC<POSE, LANDMARK, CALIBRATION> > :
170 public Testable<ProjectionFactorPPPC<POSE, LANDMARK, CALIBRATION> > {
171 };
172
173} // \ namespace gtsam
174