gtsam  4.1.0
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 
21 #pragma once
22 
24 
25 namespace gtsam {
45 
46 protected:
47 
48  std::vector<boost::shared_ptr<Cal3_S2Stereo> > K_all_;
49 
50 public:
51 
52  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
53 
56 
59 
61  typedef boost::shared_ptr<This> shared_ptr;
62 
70  const boost::optional<Pose3> body_P_sensor = boost::none) :
71  Base(sharedNoiseModel, params, body_P_sensor) {
72  }
73 
76 
83  void add(const StereoPoint2 measured, const Key poseKey,
84  const boost::shared_ptr<Cal3_S2Stereo> K) {
85  Base::add(measured, poseKey);
86  K_all_.push_back(K);
87  }
88 
95  void add(std::vector<StereoPoint2> measurements, KeyVector poseKeys,
96  std::vector<boost::shared_ptr<Cal3_S2Stereo> > Ks) {
97  Base::add(measurements, poseKeys);
98  for (size_t i = 0; i < measurements.size(); i++) {
99  K_all_.push_back(Ks.at(i));
100  }
101  }
102 
109  void add(std::vector<StereoPoint2> measurements, KeyVector poseKeys,
110  const boost::shared_ptr<Cal3_S2Stereo> K) {
111  for (size_t i = 0; i < measurements.size(); i++) {
112  Base::add(measurements.at(i), poseKeys.at(i));
113  K_all_.push_back(K);
114  }
115  }
116 
122  void print(const std::string& s = "", const KeyFormatter& keyFormatter =
123  DefaultKeyFormatter) const override {
124  std::cout << s << "SmartStereoProjectionPoseFactor, z = \n ";
125  for(const boost::shared_ptr<Cal3_S2Stereo>& K: K_all_)
126  K->print("calibration = ");
127  Base::print("", keyFormatter);
128  }
129 
131  bool equals(const NonlinearFactor& p, double tol = 1e-9) const override {
133  dynamic_cast<const SmartStereoProjectionPoseFactor*>(&p);
134 
135  return e && Base::equals(p, tol);
136  }
137 
141  double error(const Values& values) const override {
142  if (this->active(values)) {
143  return this->totalReprojectionError(cameras(values));
144  } else { // else of active flag
145  return 0.0;
146  }
147  }
148 
150  inline const std::vector<boost::shared_ptr<Cal3_S2Stereo> > calibration() const {
151  return K_all_;
152  }
153 
160  Base::Cameras cameras(const Values& values) const override {
161  Base::Cameras cameras;
162  size_t i=0;
163  for(const Key& k: this->keys_) {
164  Pose3 pose = values.at<Pose3>(k);
165 
166  if (Base::body_P_sensor_)
167  pose = pose.compose(*(Base::body_P_sensor_));
168 
169  StereoCamera camera(pose, K_all_[i++]);
170  cameras.push_back(camera);
171  }
172  return cameras;
173  }
174 
175 private:
176 
179  template<class ARCHIVE>
180  void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
181  ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base);
182  ar & BOOST_SERIALIZATION_NVP(K_all_);
183  }
184 
185 }; // end of class declaration
186 
188 template<>
190  SmartStereoProjectionPoseFactor> {
191 };
192 
193 } // \ namespace gtsam
gtsam::Factor::print
void print(const std::string &s="Factor", const KeyFormatter &formatter=DefaultKeyFormatter) const
print
Definition: Factor.cpp:29
gtsam::SmartProjectionParams
Definition: SmartFactorParams.h:42
SmartStereoProjectionFactor.h
Smart stereo factor on StereoCameras (pose + calibration)
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::StereoPoint2
Definition: StereoPoint2.h:32
gtsam::Key
std::uint64_t Key
Integer nonlinear key type.
Definition: types.h:61
gtsam::SmartStereoProjectionPoseFactor::calibration
const std::vector< boost::shared_ptr< Cal3_S2Stereo > > calibration() const
return the calibration object
Definition: SmartStereoProjectionPoseFactor.h:150
gtsam::Factor::keys_
KeyVector keys_
The keys involved in this factor.
Definition: Factor.h:72
gtsam::SmartStereoProjectionPoseFactor::add
void add(std::vector< StereoPoint2 > measurements, KeyVector poseKeys, std::vector< boost::shared_ptr< Cal3_S2Stereo > > Ks)
Variant of the previous one in which we include a set of measurements.
Definition: SmartStereoProjectionPoseFactor.h:95
gtsam::SmartFactorBase< StereoCamera >::measured
const ZVector & measured() const
return the measurements
Definition: SmartFactorBase.h:158
gtsam
Global functions in a separate testing namespace.
Definition: chartTesting.h:28
gtsam::SmartStereoProjectionFactor::totalReprojectionError
double totalReprojectionError(const Cameras &cameras, boost::optional< Point3 > externalPoint=boost::none) const
Calculate the error of the factor.
Definition: SmartStereoProjectionFactor.h:416
gtsam::SmartStereoProjectionFactor
SmartStereoProjectionFactor: triangulates point and keeps an estimate of it around.
Definition: SmartStereoProjectionFactor.h:52
gtsam::SmartStereoProjectionPoseFactor::error
double error(const Values &values) const override
error calculates the error of the factor.
Definition: SmartStereoProjectionPoseFactor.h:141
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::SmartStereoProjectionPoseFactor::~SmartStereoProjectionPoseFactor
virtual ~SmartStereoProjectionPoseFactor()
Virtual destructor.
Definition: SmartStereoProjectionPoseFactor.h:75
gtsam::SmartStereoProjectionPoseFactor::K_all_
std::vector< boost::shared_ptr< Cal3_S2Stereo > > K_all_
shared pointer to calibration object (one for each camera)
Definition: SmartStereoProjectionPoseFactor.h:48
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::Values::at
ValueType at(Key j) const
Retrieve a variable by key j.
Definition: Values-inl.h:342
gtsam::Pose3
Definition: Pose3.h:37
gtsam::StereoCamera
Definition: StereoCamera.h:47
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::SmartStereoProjectionPoseFactor::equals
bool equals(const NonlinearFactor &p, double tol=1e-9) const override
equals
Definition: SmartStereoProjectionPoseFactor.h:131
gtsam::SmartStereoProjectionPoseFactor::Base
EIGEN_MAKE_ALIGNED_OPERATOR_NEW typedef SmartStereoProjectionFactor Base
shorthand for base class type
Definition: SmartStereoProjectionPoseFactor.h:55
gtsam::Factor
This is the base class for all factor types.
Definition: Factor.h:55
gtsam::SmartStereoProjectionPoseFactor::access
friend class boost::serialization::access
Serialization function.
Definition: SmartStereoProjectionPoseFactor.h:178
gtsam::SmartStereoProjectionPoseFactor::cameras
Base::Cameras cameras(const Values &values) const override
Collect all cameras involved in this factor.
Definition: SmartStereoProjectionPoseFactor.h:160
gtsam::SmartStereoProjectionPoseFactor::print
void print(const std::string &s="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
print
Definition: SmartStereoProjectionPoseFactor.h:122
gtsam::KeyVector
FastVector< Key > KeyVector
Define collection type once and for all - also used in wrappers.
Definition: Key.h:86
gtsam::SmartStereoProjectionPoseFactor
Definition: SmartStereoProjectionPoseFactor.h:44
gtsam::Values
A non-templated config holding any types of Manifold-group elements.
Definition: Values.h:71
gtsam::Factor::equals
bool equals(const This &other, double tol=1e-9) const
check equality
Definition: Factor.cpp:42
gtsam::SmartStereoProjectionPoseFactor::This
SmartStereoProjectionPoseFactor This
shorthand for this class
Definition: SmartStereoProjectionPoseFactor.h:58
gtsam::SmartStereoProjectionPoseFactor::add
void add(std::vector< StereoPoint2 > measurements, KeyVector poseKeys, const boost::shared_ptr< Cal3_S2Stereo > K)
Variant of the previous one in which we include a set of measurements with the same noise and calibra...
Definition: SmartStereoProjectionPoseFactor.h:109
gtsam::SmartStereoProjectionPoseFactor::add
void add(const StereoPoint2 measured, const Key poseKey, const boost::shared_ptr< Cal3_S2Stereo > K)
add a new measurement and pose key
Definition: SmartStereoProjectionPoseFactor.h:83
gtsam::SmartStereoProjectionPoseFactor::shared_ptr
boost::shared_ptr< This > shared_ptr
shorthand for a smart pointer to a factor
Definition: SmartStereoProjectionPoseFactor.h:61
gtsam::SmartStereoProjectionPoseFactor::SmartStereoProjectionPoseFactor
SmartStereoProjectionPoseFactor(const SharedNoiseModel &sharedNoiseModel, const SmartStereoProjectionParams &params=SmartStereoProjectionParams(), const boost::optional< Pose3 > body_P_sensor=boost::none)
Constructor.
Definition: SmartStereoProjectionPoseFactor.h:68