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 
26 namespace gtsam {
46 class GTSAM_UNSTABLE_EXPORT SmartStereoProjectionPoseFactor
48  protected:
50  std::vector<std::shared_ptr<Cal3_S2Stereo>> K_all_;
51 
52  public:
54 
57 
60 
62  typedef std::shared_ptr<This> shared_ptr;
63 
70  const SharedNoiseModel& sharedNoiseModel,
72  const std::optional<Pose3>& body_P_sensor = {});
73 
82  void add(const StereoPoint2& measured, const Key& poseKey,
83  const std::shared_ptr<Cal3_S2Stereo>& K);
84 
93  void add(const std::vector<StereoPoint2>& measurements,
94  const KeyVector& poseKeys,
95  const std::vector<std::shared_ptr<Cal3_S2Stereo>>& Ks);
96 
106  void add(const std::vector<StereoPoint2>& measurements,
107  const KeyVector& poseKeys,
108  const std::shared_ptr<Cal3_S2Stereo>& K);
109 
115  void print(const std::string& s = "", const KeyFormatter& keyFormatter =
116  DefaultKeyFormatter) const override;
117 
119  bool equals(const NonlinearFactor& p, double tol = 1e-9) const override;
120 
124  double error(const Values& values) const override;
125 
127  inline std::vector<std::shared_ptr<Cal3_S2Stereo>> calibration() const {
128  return K_all_;
129  }
130 
138  Base::Cameras cameras(const Values& values) const override;
139 
140  private:
141 #ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
142  friend class boost::serialization::access;
144  template <class ARCHIVE>
145  void serialize(ARCHIVE& ar, const unsigned int /*version*/) {
146  ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base);
147  ar& BOOST_SERIALIZATION_NVP(K_all_);
148  }
149 #endif
150 
151 }; // end of class declaration
152 
154 template <>
156  : public Testable<SmartStereoProjectionPoseFactor> {};
157 
158 } // namespace gtsam
gtsam::add
static Y add(const Y &y1, const Y &y2)
Definition: HybridGaussianProductFactor.cpp:32
fixture::cameras
std::array< PinholeCamera< Cal3_S2 >, 3 > cameras
Definition: testTransferFactor.cpp:59
s
RealScalar s
Definition: level1_cplx_impl.h:126
e
Array< double, 1, 3 > e(1./3., 0.5, 2.)
gtsam::SmartProjectionParams
Definition: SmartFactorParams.h:42
measured
Point2 measured(-17, 30)
different_sigmas::values
HybridValues values
Definition: testHybridBayesNet.cpp:245
vanilla::params
static const SmartProjectionParams params
Definition: smartFactorScenarios.h:69
body_P_sensor
static Pose3 body_P_sensor(Rot3::RzRyRx(-M_PI_2, 0.0, -M_PI_2), Point3(0.25, -0.10, 1.0))
gtsam::KeyVector
FastVector< Key > KeyVector
Define collection type once and for all - also used in wrappers.
Definition: Key.h:92
poseKey
const gtsam::Key poseKey
Definition: testPoseRotationPrior.cpp:29
gtsam::DefaultKeyFormatter
KeyFormatter DefaultKeyFormatter
Assign default key formatter.
Definition: Key.cpp:30
gtsam::SmartStereoProjectionParams
SmartProjectionParams SmartStereoProjectionParams
Definition: SmartStereoProjectionFactor.h:45
gtsam::print
void print(const Matrix &A, const string &s, ostream &stream)
Definition: Matrix.cpp:156
gtsam::SmartStereoProjectionPoseFactor::shared_ptr
std::shared_ptr< This > shared_ptr
shorthand for a smart pointer to a factor
Definition: SmartStereoProjectionPoseFactor.h:62
gtsam::SmartStereoProjectionFactor
Definition: SmartStereoProjectionFactor.h:55
gtsam::SmartStereoProjectionPoseFactor
Definition: SmartStereoProjectionPoseFactor.h:46
gtsam::KeyFormatter
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
gtsam::SmartStereoProjectionPoseFactor::calibration
std::vector< std::shared_ptr< Cal3_S2Stereo > > calibration() const
Definition: SmartStereoProjectionPoseFactor.h:127
vanillaPose::Cameras
CameraSet< Camera > Cameras
Definition: smartFactorScenarios.h:76
gtsam::SharedNoiseModel
noiseModel::Base::shared_ptr SharedNoiseModel
Definition: NoiseModel.h:762
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
#define EIGEN_MAKE_ALIGNED_OPERATOR_NEW
Definition: Memory.h:841
gtsam::equals
Definition: Testable.h:112
gtsam::SmartStereoProjectionPoseFactor::This
SmartStereoProjectionPoseFactor This
shorthand for this class
Definition: SmartStereoProjectionPoseFactor.h:59
gtsam
traits
Definition: SFMdata.h:40
gtsam::Testable
Definition: Testable.h:152
SmartStereoProjectionFactor.h
Smart stereo factor on StereoCameras (pose)
error
static double error
Definition: testRot3.cpp:37
gtsam::traits
Definition: Group.h:36
K
#define K
Definition: igam.h:8
estimation_fixture::measurements
std::vector< double > measurements
Definition: testHybridEstimation.cpp:51
gtsam::Values
Definition: Values.h:65
gtsam::StereoPoint2
Definition: StereoPoint2.h:34
gtsam::NonlinearFactor
Definition: NonlinearFactor.h:69
p
float * p
Definition: Tutorial_Map_using.cpp:9
gtsam::SmartStereoProjectionPoseFactor::K_all_
std::vector< std::shared_ptr< Cal3_S2Stereo > > K_all_
shared pointer to calibration object (one for each camera)
Definition: SmartStereoProjectionPoseFactor.h:50
gtsam::tol
const G double tol
Definition: Group.h:79
Base
Definition: test_virtual_functions.cpp:156
gtsam::SmartStereoProjectionPoseFactor::Base
EIGEN_MAKE_ALIGNED_OPERATOR_NEW typedef SmartStereoProjectionFactor Base
shorthand for base class type
Definition: SmartStereoProjectionPoseFactor.h:56
gtsam::Key
std::uint64_t Key
Integer nonlinear key type.
Definition: types.h:97


gtsam
Author(s):
autogenerated on Sun Dec 22 2024 04:13:28