gtsam  4.1.0
gtsam
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  PinholePose<CALIBRATION> > {
47 
48 private:
52 
53 protected:
54 
55  boost::shared_ptr<CALIBRATION> K_;
56 
57 public:
58 
60  typedef boost::shared_ptr<This> shared_ptr;
61 
66 
74  const SharedNoiseModel& sharedNoiseModel,
75  const boost::shared_ptr<CALIBRATION> K,
77  : Base(sharedNoiseModel, params), K_(K) {
78  }
79 
88  const SharedNoiseModel& sharedNoiseModel,
89  const boost::shared_ptr<CALIBRATION> K,
90  const boost::optional<Pose3> body_P_sensor,
92  : SmartProjectionPoseFactor(sharedNoiseModel, K, params) {
93  this->body_P_sensor_ = body_P_sensor;
94  }
95 
98  }
99 
105  void print(const std::string& s = "", const KeyFormatter& keyFormatter =
106  DefaultKeyFormatter) const override {
107  std::cout << s << "SmartProjectionPoseFactor, z = \n ";
108  Base::print("", keyFormatter);
109  }
110 
112  bool equals(const NonlinearFactor& p, double tol = 1e-9) const override {
113  const This *e = dynamic_cast<const This*>(&p);
114  return e && Base::equals(p, tol);
115  }
116 
120  double error(const Values& values) const override {
121  if (this->active(values)) {
122  return this->totalReprojectionError(cameras(values));
123  } else { // else of active flag
124  return 0.0;
125  }
126  }
127 
129  inline const boost::shared_ptr<CALIBRATION> calibration() const {
130  return K_;
131  }
132 
139  typename Base::Cameras cameras(const Values& values) const override {
140  typename Base::Cameras cameras;
141  for (const Key& k : this->keys_) {
142  const Pose3 world_P_sensor_k =
144  : values.at<Pose3>(k);
145  cameras.emplace_back(world_P_sensor_k, K_);
146  }
147  return cameras;
148  }
149 
150  private:
151 
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 
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::SmartProjectionPoseFactor
SmartProjectionPoseFactor(const SharedNoiseModel &sharedNoiseModel, const boost::shared_ptr< CALIBRATION > K, const boost::optional< Pose3 > body_P_sensor, const SmartProjectionParams &params=SmartProjectionParams())
Constructor.
Definition: SmartProjectionPoseFactor.h:87
gtsam::SmartProjectionPoseFactor::SmartProjectionPoseFactor
SmartProjectionPoseFactor(const SharedNoiseModel &sharedNoiseModel, const boost::shared_ptr< CALIBRATION > K, const SmartProjectionParams &params=SmartProjectionParams())
Constructor.
Definition: SmartProjectionPoseFactor.h:73
gtsam::SmartProjectionPoseFactor
Definition: SmartProjectionPoseFactor.h:46
gtsam::SmartProjectionPoseFactor::K_
boost::shared_ptr< CALIBRATION > K_
calibration object (one for all cameras)
Definition: SmartProjectionPoseFactor.h:55
gtsam::SmartProjectionParams
Definition: SmartFactorParams.h:42
gtsam::SmartProjectionPoseFactor::shared_ptr
boost::shared_ptr< This > shared_ptr
shorthand for a smart pointer to a factor
Definition: SmartProjectionPoseFactor.h:60
gtsam::SharedNoiseModel
noiseModel::Base::shared_ptr SharedNoiseModel
Note, deliberately not in noiseModel namespace.
Definition: NoiseModel.h:734
gtsam::traits
A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
Definition: concepts.h:30
gtsam::Key
std::uint64_t Key
Integer nonlinear key type.
Definition: types.h:61
gtsam::Factor::keys_
KeyVector keys_
The keys involved in this factor.
Definition: Factor.h:72
gtsam
Global functions in a separate testing namespace.
Definition: chartTesting.h:28
gtsam::SmartProjectionFactor::print
void print(const std::string &s="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
print
Definition: SmartProjectionFactor.h:101
gtsam::SmartProjectionFactor::equals
bool equals(const NonlinearFactor &p, double tol=1e-9) const override
equals
Definition: SmartProjectionFactor.h:113
gtsam::Testable
A helper that implements the traits interface for GTSAM types.
Definition: Testable.h:150
gtsam::NonlinearFactor
Nonlinear factor base class.
Definition: NonlinearFactor.h:43
gtsam::SmartProjectionPoseFactor::equals
bool equals(const NonlinearFactor &p, double tol=1e-9) const override
equals
Definition: SmartProjectionPoseFactor.h:112
gtsam::NonlinearFactor::active
virtual bool active(const Values &) const
Checks whether a factor should be used based on a set of values.
Definition: NonlinearFactor.h:106
gtsam::PinholePose
Definition: PinholePose.h:237
gtsam::Values::at
ValueType at(Key j) const
Retrieve a variable by key j.
Definition: Values-inl.h:342
gtsam::SmartProjectionFactor< PinholePose< CALIBRATION > >::totalReprojectionError
double totalReprojectionError(const Cameras &cameras, boost::optional< Point3 > externalPoint=boost::none) const
Calculate the error of the factor.
Definition: SmartProjectionFactor.h:390
gtsam::Pose3
Definition: Pose3.h:37
gtsam::KeyFormatter
boost::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::Factor
This is the base class for all factor types.
Definition: Factor.h:55
gtsam::SmartProjectionPoseFactor::access
friend class boost::serialization::access
Serialization function.
Definition: SmartProjectionPoseFactor.h:153
SmartProjectionFactor.h
Smart factor on cameras (pose + calibration)
gtsam::SmartProjectionPoseFactor::error
double error(const Values &values) const override
error calculates the error of the factor.
Definition: SmartProjectionPoseFactor.h:120
gtsam::SmartProjectionPoseFactor::print
void print(const std::string &s="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
print
Definition: SmartProjectionPoseFactor.h:105
gtsam::SmartProjectionPoseFactor::cameras
Base::Cameras cameras(const Values &values) const override
Collect all cameras involved in this factor.
Definition: SmartProjectionPoseFactor.h:139
gtsam::Values
A non-templated config holding any types of Manifold-group elements.
Definition: Values.h:71
gtsam::SmartProjectionFactor
SmartProjectionFactor: triangulates point and keeps an estimate of it around.
Definition: SmartProjectionFactor.h:45
gtsam::SmartProjectionPoseFactor::calibration
const boost::shared_ptr< CALIBRATION > calibration() const
return calibration shared pointers
Definition: SmartProjectionPoseFactor.h:129
gtsam::SmartProjectionPoseFactor::~SmartProjectionPoseFactor
virtual ~SmartProjectionPoseFactor()
Virtual destructor.
Definition: SmartProjectionPoseFactor.h:97
gtsam::SmartProjectionPoseFactor::SmartProjectionPoseFactor
SmartProjectionPoseFactor()
Default constructor, only for serialization.
Definition: SmartProjectionPoseFactor.h:65
gtsam::SmartProjectionFactor::Cameras
CameraSet< CAMERA > Cameras
shorthand for a set of cameras
Definition: SmartProjectionFactor.h:73
gtsam::SmartFactorBase< PinholePose< CALIBRATION > >::body_P_sensor_
boost::optional< Pose3 > body_P_sensor_
Pose of the camera in the body frame.
Definition: SmartFactorBase.h:78