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> > {
Concept check for values that can be used in unit tests.
Base class and basic functions for Manifold types.
Access to matrices via blocks of pre-defined sizes.
typedef and functions to augment Eigen's VectorXd
typedef and functions to augment Eigen's MatrixXd
Typedefs for easier changing of 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
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...