SmartProjectionPoseFactor.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 
20 #pragma once
21 
23 
24 namespace gtsam {
44 template <class CALIBRATION>
46  : public SmartProjectionFactor<PinholePose<CALIBRATION> > {
47  private:
51 
52 protected:
53 
54  std::shared_ptr<CALIBRATION> K_;
55 
56 public:
57 
59  typedef std::shared_ptr<This> shared_ptr;
60 
65 
73  const SharedNoiseModel& sharedNoiseModel,
74  const std::shared_ptr<CALIBRATION> K,
76  : Base(sharedNoiseModel, params), K_(K) {
77  }
78 
87  const SharedNoiseModel& sharedNoiseModel,
88  const std::shared_ptr<CALIBRATION> K,
89  const std::optional<Pose3> body_P_sensor,
91  : SmartProjectionPoseFactor(sharedNoiseModel, K, params) {
93  }
94 
97  }
98 
104  void print(const std::string& s = "", const KeyFormatter& keyFormatter =
105  DefaultKeyFormatter) const override {
106  std::cout << s << "SmartProjectionPoseFactor, z = \n ";
107  Base::print("", keyFormatter);
108  }
109 
111  bool equals(const NonlinearFactor& p, double tol = 1e-9) const override {
112  const This *e = dynamic_cast<const This*>(&p);
113  return e && Base::equals(p, tol);
114  }
115 
119  double error(const Values& values) const override {
120  if (this->active(values)) {
121  return this->totalReprojectionError(cameras(values));
122  } else { // else of active flag
123  return 0.0;
124  }
125  }
126 
128  inline const std::shared_ptr<CALIBRATION> calibration() const {
129  return K_;
130  }
131 
138  typename Base::Cameras cameras(const Values& values) const override {
139  typename Base::Cameras cameras;
140  for (const Key& k : this->keys_) {
141  const Pose3 world_P_sensor_k =
143  : values.at<Pose3>(k);
144  cameras.emplace_back(world_P_sensor_k, K_);
145  }
146  return cameras;
147  }
148 
149  private:
150 
151 #ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
152  friend class boost::serialization::access;
154  template<class ARCHIVE>
155  void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
156  ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base);
157  ar & BOOST_SERIALIZATION_NVP(K_);
158  }
159 #endif
160 };
161 // end of class declaration
162 
164 template<class CALIBRATION>
165 struct traits<SmartProjectionPoseFactor<CALIBRATION> > : public Testable<
166  SmartProjectionPoseFactor<CALIBRATION> > {
167 };
168 
169 } // \ namespace gtsam
gtsam::SmartProjectionPoseFactor::K_
std::shared_ptr< CALIBRATION > K_
calibration object (one for all cameras)
Definition: SmartProjectionPoseFactor.h:54
gtsam::SmartFactorBase< PinholePose< CALIBRATION > >::body_P_sensor_
std::optional< Pose3 > body_P_sensor_
Pose of the camera in the body frame.
Definition: SmartFactorBase.h:83
gtsam::SmartFactorBase< PinholePose< CALIBRATION > >::body_P_sensor
Pose3 body_P_sensor() const
Definition: SmartFactorBase.h:440
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
SmartProjectionFactor.h
Smart factor on cameras (pose + calibration)
gtsam::SmartProjectionPoseFactor::print
void print(const std::string &s="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
Definition: SmartProjectionPoseFactor.h:104
gtsam::SmartProjectionFactor::equals
bool equals(const NonlinearFactor &p, double tol=1e-9) const override
equals
Definition: SmartProjectionFactor.h:114
different_sigmas::values
HybridValues values
Definition: testHybridBayesNet.cpp:245
gtsam::Factor
Definition: Factor.h:70
vanilla::params
static const SmartProjectionParams params
Definition: smartFactorScenarios.h:69
gtsam::SmartProjectionPoseFactor::cameras
Base::Cameras cameras(const Values &values) const override
Definition: SmartProjectionPoseFactor.h:138
gtsam::SmartProjectionFactor< PinholePose< CALIBRATION > >::totalReprojectionError
double totalReprojectionError(const Cameras &cameras, std::optional< Point3 > externalPoint={}) const
Definition: SmartProjectionFactor.h:411
gtsam::NonlinearFactor::active
virtual bool active(const Values &) const
Definition: NonlinearFactor.h:141
gtsam::SmartProjectionPoseFactor::equals
bool equals(const NonlinearFactor &p, double tol=1e-9) const override
equals
Definition: SmartProjectionPoseFactor.h:111
gtsam::DefaultKeyFormatter
KeyFormatter DefaultKeyFormatter
Assign default key formatter.
Definition: Key.cpp:30
gtsam::SmartProjectionFactor::Cameras
CameraSet< CAMERA > Cameras
Definition: SmartProjectionFactor.h:74
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::Pose3
Definition: Pose3.h:37
gtsam::SmartProjectionPoseFactor
Definition: SmartProjectionPoseFactor.h:45
gtsam::SmartProjectionPoseFactor::~SmartProjectionPoseFactor
~SmartProjectionPoseFactor() override
Definition: SmartProjectionPoseFactor.h:96
gtsam::SmartProjectionPoseFactor::calibration
const std::shared_ptr< CALIBRATION > calibration() const
Definition: SmartProjectionPoseFactor.h:128
gtsam::SmartProjectionPoseFactor::SmartProjectionPoseFactor
SmartProjectionPoseFactor(const SharedNoiseModel &sharedNoiseModel, const std::shared_ptr< CALIBRATION > K, const SmartProjectionParams &params=SmartProjectionParams())
Definition: SmartProjectionPoseFactor.h:72
gtsam::SharedNoiseModel
noiseModel::Base::shared_ptr SharedNoiseModel
Definition: NoiseModel.h:762
gtsam::SmartProjectionFactor::print
void print(const std::string &s="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
Definition: SmartProjectionFactor.h:102
gtsam::SmartProjectionFactor
Definition: SmartProjectionFactor.h:44
gtsam
traits
Definition: SFMdata.h:40
gtsam::Testable
Definition: Testable.h:152
gtsam::Factor::keys_
KeyVector keys_
The keys involved in this factor.
Definition: Factor.h:88
gtsam::traits
Definition: Group.h:36
K
#define K
Definition: igam.h:8
gtsam::Values
Definition: Values.h:65
gtsam::NonlinearFactor
Definition: NonlinearFactor.h:68
p
float * p
Definition: Tutorial_Map_using.cpp:9
gtsam::tol
const G double tol
Definition: Group.h:79
gtsam::SmartProjectionPoseFactor::Base
SmartProjectionFactor< Camera > Base
Definition: SmartProjectionPoseFactor.h:49
gtsam::SmartProjectionPoseFactor::Camera
PinholePose< CALIBRATION > Camera
Definition: SmartProjectionPoseFactor.h:48
gtsam::SmartProjectionPoseFactor::shared_ptr
std::shared_ptr< This > shared_ptr
shorthand for a smart pointer to a factor
Definition: SmartProjectionPoseFactor.h:59
gtsam::SmartProjectionPoseFactor::SmartProjectionPoseFactor
SmartProjectionPoseFactor(const SharedNoiseModel &sharedNoiseModel, const std::shared_ptr< CALIBRATION > K, const std::optional< Pose3 > body_P_sensor, const SmartProjectionParams &params=SmartProjectionParams())
Definition: SmartProjectionPoseFactor.h:86
Base
Definition: test_virtual_functions.cpp:156
gtsam::SmartProjectionPoseFactor::SmartProjectionPoseFactor
SmartProjectionPoseFactor()
Definition: SmartProjectionPoseFactor.h:64
gtsam::PinholePose
Definition: PinholePose.h:239
gtsam::Key
std::uint64_t Key
Integer nonlinear key type.
Definition: types.h:97
gtsam::SmartProjectionPoseFactor::error
double error(const Values &values) const override
Definition: SmartProjectionPoseFactor.h:119
gtsam::SmartProjectionPoseFactor::This
SmartProjectionPoseFactor< CALIBRATION > This
Definition: SmartProjectionPoseFactor.h:50


gtsam
Author(s):
autogenerated on Fri Nov 1 2024 03:35:30