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");
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) {
169 SharedDiagonal model;
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);
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> > {
typedef and functions to augment Eigen's MatrixXd
Typedefs for easier changing of types.
Access to matrices via blocks of pre-defined sizes.
typedef and functions to augment Eigen's VectorXd
Base class and basic functions for Manifold types.
Concept check for values that can be used in unit tests.
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
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
A 3D pose (R,t) : (Rot3,Point3)
Definition: Pose3.h:37
Nonlinear factor base class.
Definition: NonlinearFactor.h:42
NonlinearFactor()
Default constructor for I/O only.
Definition: NonlinearFactor.h:58
virtual bool active(const Values &) const
Checks whether a factor should be used based on a set of values.
Definition: NonlinearFactor.h:118
void print(const std::string &s="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
Print.
Definition: NonlinearFactor.cpp:74
bool equals(const NonlinearFactor &f, double tol=1e-9) const override
Check if two factors are equal.
Definition: NonlinearFactor.cpp:82
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...