30#include <gtsam/base/concepts.h>
39#include <boost/none.hpp>
40#include <boost/optional/optional.hpp>
41#include <boost/serialization/nvp.hpp>
42#include <boost/smart_ptr/shared_ptr.hpp>
47namespace serialization {
59template<
class CAMERA,
class LANDMARK>
62 GTSAM_CONCEPT_MANIFOLD_TYPE(CAMERA)
63 GTSAM_CONCEPT_MANIFOLD_TYPE(LANDMARK)
67 typedef Eigen::Matrix<double, 2, DimC> JacobianC;
68 typedef Eigen::Matrix<double, 2, DimL> JacobianL;
80 typedef boost::shared_ptr<This> shared_ptr;
90 Key cameraKey,
Key landmarkKey)
102 gtsam::NonlinearFactor::shared_ptr
clone()
const override {
103 return boost::static_pointer_cast<gtsam::NonlinearFactor>(
104 gtsam::NonlinearFactor::shared_ptr(
new This(*
this)));}
111 void print(
const std::string& s =
"SFMFactor",
const KeyFormatter& keyFormatter = DefaultKeyFormatter)
const override {
113 traits<Point2>::Print(
measured_, s +
".z");
119 bool equals(
const NonlinearFactor &p,
double tol = 1e-9)
const override {
120 const This* e =
dynamic_cast<const This*
>(&p);
121 return e &&
Base::equals(p, tol) && traits<Point2>::Equals(this->measured_, e->measured_, tol);
125 Vector
evaluateError(
const CAMERA& camera,
const LANDMARK& point,
126 boost::optional<Matrix&> H1=boost::none, boost::optional<Matrix&> H2=boost::none)
const override {
128 return camera.project2(point,H1,H2) -
measured_;
130 catch( CheiralityException& e) {
131 if (H1) *H1 = JacobianC::Zero();
132 if (H2) *H2 = JacobianL::Zero();
139 boost::shared_ptr<GaussianFactor>
linearize(
const Values& values)
const override {
141 if (!this->
active(values))
return boost::shared_ptr<JacobianFactor>();
143 const Key key1 = this->key1(), key2 = this->key2();
148 const CAMERA& camera = values.at<CAMERA>(key1);
149 const LANDMARK& point = values.at<LANDMARK>(key2);
150 b =
measured() - camera.project2(point, H1, H2);
151 }
catch (CheiralityException& e) {
160 if (noiseModel && !noiseModel->isUnit()) {
163 H1 = noiseModel->Whiten(H1);
164 H2 = noiseModel->Whiten(H2);
165 b = noiseModel->Whiten(b);
169 SharedDiagonal model;
170 if (noiseModel && noiseModel->isConstrained()) {
171 model = boost::static_pointer_cast<noiseModel::Constrained>(noiseModel)->unit();
174 return boost::make_shared<BinaryJacobianFactor<2, DimC, DimL> >(key1, H1, key2, H2, b, model);
185 template<
class Archive>
186 void serialize(Archive & ar,
const unsigned int ) {
188 ar & boost::serialization::make_nvp(
"NoiseModelFactor2",
189 boost::serialization::base_object<Base>(*
this));
194template<
class CAMERA,
class LANDMARK>
196 GeneralSFMFactor<CAMERA, LANDMARK> > {
203template<
class CALIBRATION>
206 GTSAM_CONCEPT_MANIFOLD_TYPE(CALIBRATION)
220 typedef boost::shared_ptr<This> shared_ptr;
237 gtsam::NonlinearFactor::shared_ptr
clone()
const override {
238 return boost::static_pointer_cast<gtsam::NonlinearFactor>(
239 gtsam::NonlinearFactor::shared_ptr(
new This(*
this)));}
246 void print(
const std::string& s =
"SFMFactor2",
const KeyFormatter& keyFormatter = DefaultKeyFormatter)
const override {
255 const This* e =
dynamic_cast<const This*
>(&p);
261 boost::optional<Matrix&> H1=boost::none,
262 boost::optional<Matrix&> H2=boost::none,
263 boost::optional<Matrix&> H3=boost::none)
const override
266 Camera camera(pose3,calib);
267 return camera.project(point, H1, H2, H3) -
measured_;
270 if (H1) *H1 = Matrix::Zero(2, 6);
271 if (H2) *H2 = Matrix::Zero(2, 3);
272 if (H3) *H3 = Matrix::Zero(2, DimK);
273 std::cout << e.what() <<
": Landmark "<< DefaultKeyFormatter(this->key2())
274 <<
" behind Camera " << DefaultKeyFormatter(this->key1()) << std::endl;
287 template<
class Archive>
288 void serialize(Archive & ar,
const unsigned int ) {
290 ar & boost::serialization::make_nvp(
"NoiseModelFactor3",
291 boost::serialization::base_object<Base>(*
this));
296template<
class CALIBRATION>
298 GeneralSFMFactor2<CALIBRATION> > {
Typedefs for easier changing of types.
typedef and functions to augment Eigen's VectorXd
Concept check for values that can be used in unit tests.
Access to matrices via blocks of pre-defined sizes.
typedef and functions to augment Eigen's MatrixXd
Base class and basic functions for Manifold types.
Base class for all pinhole cameras.
A binary JacobianFactor specialization that uses fixed matrix math for speed.
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:36
noiseModel::Base::shared_ptr SharedNoiseModel
Aliases.
Definition NoiseModel.h:724
std::uint64_t Key
Integer nonlinear key type.
Definition types.h:100
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
Give fixed size dimension of a type, fails at compile time if dynamic.
Definition Manifold.h:164
A helper that implements the traits interface for GTSAM types.
Definition Testable.h:151
Definition CalibratedCamera.h:32
A pinhole camera class that has a Pose3 and a Calibration.
Definition PinholeCamera.h:33
A 3D pose (R,t) : (Rot3,Point3)
Definition Pose3.h:37
virtual void print(const std::string &s="Factor", const KeyFormatter &formatter=DefaultKeyFormatter) const
print
Definition Factor.cpp:29
bool equals(const This &other, double tol=1e-9) const
check equality
Definition Factor.cpp:42
Nonlinear factor base class.
Definition NonlinearFactor.h:42
virtual bool active(const Values &) const
Checks whether a factor should be used based on a set of values.
Definition NonlinearFactor.h:118
const SharedNoiseModel & noiseModel() const
access to the noise model
Definition NonlinearFactor.h:223
A convenient base class for creating your own NoiseModelFactor with n variables.
Definition NonlinearFactor.h:400
Non-linear factor for a constraint derived from a 2D measurement.
Definition GeneralSFMFactor.h:60
boost::shared_ptr< GaussianFactor > linearize(const Values &values) const override
Linearize using fixed-size matrices.
Definition GeneralSFMFactor.h:138
Vector evaluateError(const CAMERA &camera, const LANDMARK &point, boost::optional< Matrix & > H1=boost::none, boost::optional< Matrix & > H2=boost::none) const override
h(x)-z
Definition GeneralSFMFactor.h:124
gtsam::NonlinearFactor::shared_ptr clone() const override
Definition GeneralSFMFactor.h:101
bool equals(const NonlinearFactor &p, double tol=1e-9) const override
equals
Definition GeneralSFMFactor.h:118
const Point2 measured() const
return the measured
Definition GeneralSFMFactor.h:177
GeneralSFMFactor(const Point2 &measured, const SharedNoiseModel &model, Key cameraKey, Key landmarkKey)
Constructor.
Definition GeneralSFMFactor.h:89
GeneralSFMFactor()
default constructor
Definition GeneralSFMFactor.h:93
GeneralSFMFactor< CAMERA, LANDMARK > This
typedef for this object
Definition GeneralSFMFactor.h:76
~GeneralSFMFactor() override
destructor
Definition GeneralSFMFactor.h:98
NoiseModelFactorN< CAMERA, LANDMARK > Base
typedef for the base class
Definition GeneralSFMFactor.h:77
void print(const std::string &s="SFMFactor", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
print
Definition GeneralSFMFactor.h:110
friend class boost::serialization::access
Serialization function.
Definition GeneralSFMFactor.h:183
Point2 measured_
the 2D measurement
Definition GeneralSFMFactor.h:72
Non-linear factor for a constraint derived from a 2D measurement.
Definition GeneralSFMFactor.h:204
GeneralSFMFactor2()
default constructor
Definition GeneralSFMFactor.h:232
~GeneralSFMFactor2() override
destructor
Definition GeneralSFMFactor.h:234
gtsam::NonlinearFactor::shared_ptr clone() const override
Definition GeneralSFMFactor.h:237
NoiseModelFactorN< Pose3, Point3, CALIBRATION > Base
typedef for the base class
Definition GeneralSFMFactor.h:217
Vector evaluateError(const Pose3 &pose3, const Point3 &point, const CALIBRATION &calib, boost::optional< Matrix & > H1=boost::none, boost::optional< Matrix & > H2=boost::none, boost::optional< Matrix & > H3=boost::none) const override
h(x)-z
Definition GeneralSFMFactor.h:260
void print(const std::string &s="SFMFactor2", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
print
Definition GeneralSFMFactor.h:246
Point2 measured_
the 2D measurement
Definition GeneralSFMFactor.h:211
const Point2 measured() const
return the measured
Definition GeneralSFMFactor.h:280
PinholeCamera< CALIBRATION > Camera
typedef for camera type
Definition GeneralSFMFactor.h:216
GeneralSFMFactor2(const Point2 &measured, const SharedNoiseModel &model, Key poseKey, Key landmarkKey, Key calibKey)
Constructor.
Definition GeneralSFMFactor.h:230
bool equals(const NonlinearFactor &p, double tol=1e-9) const override
equals
Definition GeneralSFMFactor.h:254
friend class boost::serialization::access
Serialization function.
Definition GeneralSFMFactor.h:286
In nonlinear factors, the error function returns the negative log-likelihood as a non-linear function...