gtsam 4.1.1
gtsam
SmartStereoProjectionPoseFactor.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
22#pragma once
23
25
26namespace gtsam {
47 protected:
49 std::vector<boost::shared_ptr<Cal3_S2Stereo>> K_all_;
50
51 public:
52 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
53
56
59
61 typedef boost::shared_ptr<This> shared_ptr;
62
69 const SharedNoiseModel& sharedNoiseModel,
71 const boost::optional<Pose3>& body_P_sensor = boost::none);
72
75
84 void add(const StereoPoint2& measured, const Key& poseKey,
85 const boost::shared_ptr<Cal3_S2Stereo>& K);
86
95 void add(const std::vector<StereoPoint2>& measurements,
96 const KeyVector& poseKeys,
97 const std::vector<boost::shared_ptr<Cal3_S2Stereo>>& Ks);
98
108 void add(const std::vector<StereoPoint2>& measurements,
109 const KeyVector& poseKeys,
110 const boost::shared_ptr<Cal3_S2Stereo>& K);
111
117 void print(const std::string& s = "", const KeyFormatter& keyFormatter =
118 DefaultKeyFormatter) const override;
119
121 bool equals(const NonlinearFactor& p, double tol = 1e-9) const override;
122
126 double error(const Values& values) const override;
127
129 inline std::vector<boost::shared_ptr<Cal3_S2Stereo>> calibration() const {
130 return K_all_;
131 }
132
140 Base::Cameras cameras(const Values& values) const override;
141
142 private:
145 template <class ARCHIVE>
146 void serialize(ARCHIVE& ar, const unsigned int /*version*/) {
147 ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base);
148 ar& BOOST_SERIALIZATION_NVP(K_all_);
149 }
150
151}; // end of class declaration
152
154template <>
156 : public Testable<SmartStereoProjectionPoseFactor> {};
157
158} // namespace gtsam
Smart stereo factor on StereoCameras (pose)
Global functions in a separate testing namespace.
Definition: chartTesting.h:28
FastVector< Key > KeyVector
Define collection type once and for all - also used in wrappers.
Definition: Key.h:86
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: StereoPoint2.h:32
This is the base class for all factor types.
Definition: Factor.h:56
Nonlinear factor base class.
Definition: NonlinearFactor.h:43
A non-templated config holding any types of Manifold-group elements.
Definition: Values.h:63
const ZVector & measured() const
Return the 2D measurements (ZDim, in general).
Definition: SmartFactorBase.h:159
Definition: SmartFactorParams.h:42
SmartStereoProjectionFactor: triangulates point and keeps an estimate of it around.
Definition: SmartStereoProjectionFactor.h:52
Definition: SmartStereoProjectionPoseFactor.h:46
SmartStereoProjectionPoseFactor This
shorthand for this class
Definition: SmartStereoProjectionPoseFactor.h:58
double error(const Values &values) const override
error calculates the error of the factor.
Definition: SmartStereoProjectionPoseFactor.cpp:75
void print(const std::string &s="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
print
Definition: SmartStereoProjectionPoseFactor.cpp:58
void add(const StereoPoint2 &measured, const Key &poseKey, const boost::shared_ptr< Cal3_S2Stereo > &K)
add a new measurement and pose key
Definition: SmartStereoProjectionPoseFactor.cpp:32
std::vector< boost::shared_ptr< Cal3_S2Stereo > > calibration() const
return the calibration object
Definition: SmartStereoProjectionPoseFactor.h:129
Base::Cameras cameras(const Values &values) const override
Collect all cameras involved in this factor.
Definition: SmartStereoProjectionPoseFactor.cpp:84
~SmartStereoProjectionPoseFactor() override=default
Virtual destructor.
EIGEN_MAKE_ALIGNED_OPERATOR_NEW typedef SmartStereoProjectionFactor Base
shorthand for base class type
Definition: SmartStereoProjectionPoseFactor.h:55
boost::shared_ptr< This > shared_ptr
shorthand for a smart pointer to a factor
Definition: SmartStereoProjectionPoseFactor.h:61
SmartStereoProjectionPoseFactor(const SharedNoiseModel &sharedNoiseModel, const SmartStereoProjectionParams &params=SmartStereoProjectionParams(), const boost::optional< Pose3 > &body_P_sensor=boost::none)
Constructor.
Definition: SmartStereoProjectionPoseFactor.cpp:26
bool equals(const NonlinearFactor &p, double tol=1e-9) const override
equals
Definition: SmartStereoProjectionPoseFactor.cpp:67
friend class boost::serialization::access
Serialization function.
Definition: SmartStereoProjectionPoseFactor.h:144
std::vector< boost::shared_ptr< Cal3_S2Stereo > > K_all_
shared pointer to calibration object (one for each camera)
Definition: SmartStereoProjectionPoseFactor.h:49