gtsam 4.1.1
gtsam
ProjectionFactorPPPC.h
Go to the documentation of this file.
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
19#pragma once
20
24#include <boost/optional.hpp>
25
26namespace gtsam {
27
33 template<class POSE, class LANDMARK, class CALIBRATION = Cal3_S2>
34 class ProjectionFactorPPPC: public NoiseModelFactor4<POSE, POSE, LANDMARK, CALIBRATION> {
35 protected:
36
38
39 // verbosity handling for Cheirality Exceptions
42
43 public:
44
47
50
52 typedef boost::shared_ptr<This> shared_ptr;
53
56 measured_(0.0, 0.0), throwCheirality_(false), verboseCheirality_(false) {
57 }
58
69 Key poseKey, Key transformKey, Key pointKey, Key calibKey) :
70 Base(model, poseKey, transformKey, pointKey, calibKey), measured_(measured),
71 throwCheirality_(false), verboseCheirality_(false) {}
72
85 Key poseKey, Key transformKey, Key pointKey, Key calibKey,
87 Base(model, poseKey, transformKey, pointKey, calibKey), measured_(measured),
89
92
94 NonlinearFactor::shared_ptr clone() const override {
95 return boost::static_pointer_cast<NonlinearFactor>(
96 NonlinearFactor::shared_ptr(new This(*this))); }
97
103 void print(const std::string& s = "", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override {
104 std::cout << s << "ProjectionFactorPPPC, z = ";
106 Base::print("", keyFormatter);
107 }
108
110 bool equals(const NonlinearFactor& p, double tol = 1e-9) const override {
111 const This *e = dynamic_cast<const This*>(&p);
112 return e
113 && Base::equals(p, tol)
114 && traits<Point2>::Equals(this->measured_, e->measured_, tol);
115 }
116
118 Vector evaluateError(const Pose3& pose, const Pose3& transform, const Point3& point, const CALIBRATION& K,
119 boost::optional<Matrix&> H1 = boost::none,
120 boost::optional<Matrix&> H2 = boost::none,
121 boost::optional<Matrix&> H3 = boost::none,
122 boost::optional<Matrix&> H4 = boost::none) const override {
123 try {
124 if(H1 || H2 || H3 || H4) {
125 Matrix H0, H02;
126 PinholeCamera<CALIBRATION> camera(pose.compose(transform, H0, H02), K);
127 Point2 reprojectionError(camera.project(point, H1, H3, H4) - measured_);
128 *H2 = *H1 * H02;
129 *H1 = *H1 * H0;
130 return reprojectionError;
131 } else {
132 PinholeCamera<CALIBRATION> camera(pose.compose(transform), K);
133 return camera.project(point, H1, H3, H4) - measured_;
134 }
135 } catch( CheiralityException& e) {
136 if (H1) *H1 = Matrix::Zero(2,6);
137 if (H2) *H2 = Matrix::Zero(2,6);
138 if (H3) *H3 = Matrix::Zero(2,3);
139 if (H4) *H4 = Matrix::Zero(2,CALIBRATION::Dim());
141 std::cout << e.what() << ": Landmark "<< DefaultKeyFormatter(this->key2()) <<
142 " moved behind camera " << DefaultKeyFormatter(this->key1()) << std::endl;
144 throw e;
145 }
146 return Vector::Ones(2) * 2.0 * K.fx();
147 }
148
150 const Point2& measured() const {
151 return measured_;
152 }
153
155 inline bool verboseCheirality() const { return verboseCheirality_; }
156
158 inline bool throwCheirality() const { return throwCheirality_; }
159
160 private:
161
164 template<class ARCHIVE>
165 void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
166 ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base);
167 ar & BOOST_SERIALIZATION_NVP(measured_);
168 ar & BOOST_SERIALIZATION_NVP(throwCheirality_);
169 ar & BOOST_SERIALIZATION_NVP(verboseCheirality_);
170 }
171 };
172
174 template<class POSE, class LANDMARK, class CALIBRATION>
175 struct traits<ProjectionFactorPPPC<POSE, LANDMARK, CALIBRATION> > :
176 public Testable<ProjectionFactorPPPC<POSE, LANDMARK, CALIBRATION> > {
177 };
178
179} // \ namespace gtsam
The most common 5DOF 3D->2D calibration.
Base class for all pinhole cameras.
Non-linear factor base classes.
Global functions in a separate testing namespace.
Definition: chartTesting.h:28
Vector2 Point2
As of GTSAM 4, in order to make GTSAM more lean, it is now possible to just typedef Point2 to Vector2...
Definition: Point2.h:27
Vector3 Point3
As of GTSAM 4, in order to make GTSAM more lean, it is now possible to just typedef Point3 to Vector3...
Definition: Point3.h:35
noiseModel::Base::shared_ptr SharedNoiseModel
Note, deliberately not in noiseModel namespace.
Definition: NoiseModel.h:736
std::uint64_t Key
Integer nonlinear key type.
Definition: types.h:69
std::function< std::string(Key)> KeyFormatter
Typedef for a function to format a key, i.e. to convert it to a string.
Definition: Key.h:35
A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
Definition: concepts.h:30
A helper that implements the traits interface for GTSAM types.
Definition: Testable.h:151
Definition: CalibratedCamera.h:32
Definition: PinholeCamera.h:33
Point2 project(const Point3 &pw, OptionalJacobian< 2, 6 > Dpose=boost::none, OptionalJacobian< 2, 3 > Dpoint=boost::none, OptionalJacobian< 2, DimK > Dcal=boost::none) const
project a 3D point from world coordinates into the image
Definition: PinholePose.h:118
Definition: Pose3.h:37
This is the base class for all factor types.
Definition: Factor.h:56
Nonlinear factor base class.
Definition: NonlinearFactor.h:43
bool equals(const NonlinearFactor &f, double tol=1e-9) const override
Check if two factors are equal.
Definition: NonlinearFactor.cpp:71
void print(const std::string &s="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
Print.
Definition: NonlinearFactor.cpp:63
A convenient base class for creating your own NoiseModelFactor with 4 variables.
Definition: NonlinearFactor.h:521
Key key1() const
methods to retrieve keys
Definition: NonlinearFactor.h:557
Definition: ProjectionFactorPPPC.h:34
ProjectionFactorPPPC< POSE, LANDMARK, CALIBRATION > This
shorthand for this class
Definition: ProjectionFactorPPPC.h:49
const Point2 & measured() const
return the measurement
Definition: ProjectionFactorPPPC.h:150
NonlinearFactor::shared_ptr clone() const override
Definition: ProjectionFactorPPPC.h:94
boost::shared_ptr< This > shared_ptr
shorthand for a smart pointer to a factor
Definition: ProjectionFactorPPPC.h:52
bool equals(const NonlinearFactor &p, double tol=1e-9) const override
equals
Definition: ProjectionFactorPPPC.h:110
bool verboseCheirality_
If true, prints text for Cheirality exceptions (default: false)
Definition: ProjectionFactorPPPC.h:41
ProjectionFactorPPPC()
Default constructor.
Definition: ProjectionFactorPPPC.h:55
ProjectionFactorPPPC(const Point2 &measured, const SharedNoiseModel &model, Key poseKey, Key transformKey, Key pointKey, Key calibKey, bool throwCheirality, bool verboseCheirality)
Constructor with exception-handling flags TODO: Mark argument order standard (keys,...
Definition: ProjectionFactorPPPC.h:84
void print(const std::string &s="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
print
Definition: ProjectionFactorPPPC.h:103
bool throwCheirality_
If true, rethrows Cheirality exceptions (default: false)
Definition: ProjectionFactorPPPC.h:40
NoiseModelFactor4< POSE, POSE, LANDMARK, CALIBRATION > Base
shorthand for base class type
Definition: ProjectionFactorPPPC.h:46
Point2 measured_
2D measurement
Definition: ProjectionFactorPPPC.h:37
bool verboseCheirality() const
return verbosity
Definition: ProjectionFactorPPPC.h:155
bool throwCheirality() const
return flag for throwing cheirality exceptions
Definition: ProjectionFactorPPPC.h:158
friend class boost::serialization::access
Serialization function.
Definition: ProjectionFactorPPPC.h:163
~ProjectionFactorPPPC() override
Virtual destructor.
Definition: ProjectionFactorPPPC.h:91
Vector evaluateError(const Pose3 &pose, const Pose3 &transform, const Point3 &point, const CALIBRATION &K, boost::optional< Matrix & > H1=boost::none, boost::optional< Matrix & > H2=boost::none, boost::optional< Matrix & > H3=boost::none, boost::optional< Matrix & > H4=boost::none) const override
Evaluate error h(x)-z and optionally derivatives.
Definition: ProjectionFactorPPPC.h:118
ProjectionFactorPPPC(const Point2 &measured, const SharedNoiseModel &model, Key poseKey, Key transformKey, Key pointKey, Key calibKey)
Constructor TODO: Mark argument order standard (keys, measurement, parameters)
Definition: ProjectionFactorPPPC.h:68