gtsam 4.1.1
gtsam
SmartProjectionRigFactor.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
25#pragma once
26
28
29namespace gtsam {
51template <class CAMERA>
53 private:
56 typedef typename CAMERA::CalibrationType CALIBRATION;
57
58 static const int DimPose = 6;
59 static const int ZDim = 2;
60
61 protected:
64
66 boost::shared_ptr<typename Base::Cameras> cameraRig_;
67
70 FastVector<size_t> cameraIds_;
71
72 public:
73 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
74
75 typedef CAMERA Camera;
77
79 typedef boost::shared_ptr<This> shared_ptr;
80
83
93 const SharedNoiseModel& sharedNoiseModel,
94 const boost::shared_ptr<Cameras>& cameraRig,
96 : Base(sharedNoiseModel, params), cameraRig_(cameraRig) {
97 // throw exception if configuration is not supported by this factor
98 if (Base::params_.degeneracyMode != gtsam::ZERO_ON_DEGENERACY)
99 throw std::runtime_error(
100 "SmartProjectionRigFactor: "
101 "degeneracyMode must be set to ZERO_ON_DEGENERACY");
102 if (Base::params_.linearizationMode != gtsam::HESSIAN)
103 throw std::runtime_error(
104 "SmartProjectionRigFactor: "
105 "linearizationMode must be set to HESSIAN");
106 }
107
109 ~SmartProjectionRigFactor() override = default;
110
121 void add(const Point2& measured, const Key& poseKey,
122 const size_t& cameraId = 0) {
123 // store measurement and key
124 this->measured_.push_back(measured);
125 this->nonUniqueKeys_.push_back(poseKey);
126
127 // also store keys in the keys_ vector: these keys are assumed to be
128 // unique, so we avoid duplicates here
129 if (std::find(this->keys_.begin(), this->keys_.end(), poseKey) ==
130 this->keys_.end())
131 this->keys_.push_back(poseKey); // add only unique keys
132
133 // store id of the camera taking the measurement
134 cameraIds_.push_back(cameraId);
135 }
136
147 void add(const Point2Vector& measurements, const KeyVector& poseKeys,
148 const FastVector<size_t>& cameraIds = FastVector<size_t>()) {
149 if (poseKeys.size() != measurements.size() ||
150 (poseKeys.size() != cameraIds.size() && cameraIds.size() != 0)) {
151 throw std::runtime_error(
152 "SmartProjectionRigFactor: "
153 "trying to add inconsistent inputs");
154 }
155 if (cameraIds.size() == 0 && cameraRig_->size() > 1) {
156 throw std::runtime_error(
157 "SmartProjectionRigFactor: "
158 "camera rig includes multiple camera "
159 "but add did not input cameraIds");
160 }
161 for (size_t i = 0; i < measurements.size(); i++) {
162 add(measurements[i], poseKeys[i],
163 cameraIds.size() == 0 ? 0 : cameraIds[i]);
164 }
165 }
166
169 const KeyVector& nonUniqueKeys() const { return nonUniqueKeys_; }
170
172 const boost::shared_ptr<Cameras>& cameraRig() const { return cameraRig_; }
173
175 const FastVector<size_t>& cameraIds() const { return cameraIds_; }
176
182 void print(
183 const std::string& s = "",
184 const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override {
185 std::cout << s << "SmartProjectionRigFactor: \n ";
186 for (size_t i = 0; i < nonUniqueKeys_.size(); i++) {
187 std::cout << "-- Measurement nr " << i << std::endl;
188 std::cout << "key: " << keyFormatter(nonUniqueKeys_[i]) << std::endl;
189 std::cout << "cameraId: " << cameraIds_[i] << std::endl;
190 (*cameraRig_)[cameraIds_[i]].print("camera in rig:\n");
191 }
192 Base::print("", keyFormatter);
193 }
194
196 bool equals(const NonlinearFactor& p, double tol = 1e-9) const override {
197 const This* e = dynamic_cast<const This*>(&p);
198 return e && Base::equals(p, tol) && nonUniqueKeys_ == e->nonUniqueKeys() &&
199 cameraRig_->equals(*(e->cameraRig())) &&
200 std::equal(cameraIds_.begin(), cameraIds_.end(),
201 e->cameraIds().begin());
202 }
203
210 typename Base::Cameras cameras(const Values& values) const override {
211 typename Base::Cameras cameras;
212 cameras.reserve(nonUniqueKeys_.size()); // preallocate
213 for (size_t i = 0; i < nonUniqueKeys_.size(); i++) {
214 const typename Base::Camera& camera_i = (*cameraRig_)[cameraIds_[i]];
215 const Pose3 world_P_sensor_i =
216 values.at<Pose3>(nonUniqueKeys_[i]) // = world_P_body
217 * camera_i.pose(); // = body_P_cam_i
218 cameras.emplace_back(world_P_sensor_i,
219 make_shared<typename CAMERA::CalibrationType>(
220 camera_i.calibration()));
221 }
222 return cameras;
223 }
224
228 double error(const Values& values) const override {
229 if (this->active(values)) {
230 return this->totalReprojectionError(this->cameras(values));
231 } else { // else of active flag
232 return 0.0;
233 }
234 }
235
245 void computeJacobiansWithTriangulatedPoint(typename Base::FBlocks& Fs,
246 Matrix& E, Vector& b,
247 const Cameras& cameras) const {
248 if (!this->result_) {
249 throw("computeJacobiansWithTriangulatedPoint");
250 } else { // valid result: compute jacobians
251 b = -cameras.reprojectionError(*this->result_, this->measured_, Fs, E);
252 for (size_t i = 0; i < Fs.size(); i++) {
253 const Pose3& body_P_sensor = (*cameraRig_)[cameraIds_[i]].pose();
254 const Pose3 world_P_body = cameras[i].pose() * body_P_sensor.inverse();
255 Eigen::Matrix<double, DimPose, DimPose> H;
256 world_P_body.compose(body_P_sensor, H);
257 Fs.at(i) = Fs.at(i) * H;
258 }
259 }
260 }
261
263 boost::shared_ptr<RegularHessianFactor<DimPose> > createHessianFactor(
264 const Values& values, const double& lambda = 0.0,
265 bool diagonalDamping = false) const {
266 // we may have multiple observation sharing the same keys (e.g., 2 cameras
267 // measuring from the same body pose), hence the number of unique keys may
268 // be smaller than nrMeasurements
269 size_t nrUniqueKeys =
270 this->keys_
271 .size(); // note: by construction, keys_ only contains unique keys
272
273 Cameras cameras = this->cameras(values);
274
275 // Create structures for Hessian Factors
276 std::vector<size_t> js;
277 std::vector<Matrix> Gs(nrUniqueKeys * (nrUniqueKeys + 1) / 2);
278 std::vector<Vector> gs(nrUniqueKeys);
279
280 if (this->measured_.size() != cameras.size()) // 1 observation per camera
281 throw std::runtime_error(
282 "SmartProjectionRigFactor: "
283 "measured_.size() inconsistent with input");
284
285 // triangulate 3D point at given linearization point
286 this->triangulateSafe(cameras);
287
288 if (!this->result_) { // failed: return "empty/zero" Hessian
289 if (this->params_.degeneracyMode == ZERO_ON_DEGENERACY) {
290 for (Matrix& m : Gs) m = Matrix::Zero(DimPose, DimPose);
291 for (Vector& v : gs) v = Vector::Zero(DimPose);
292 return boost::make_shared<RegularHessianFactor<DimPose> >(this->keys_,
293 Gs, gs, 0.0);
294 } else {
295 throw std::runtime_error(
296 "SmartProjectionRigFactor: "
297 "only supported degeneracy mode is ZERO_ON_DEGENERACY");
298 }
299 }
300
301 // compute Jacobian given triangulated 3D Point
302 typename Base::FBlocks Fs;
303 Matrix E;
304 Vector b;
306
307 // Whiten using noise model
308 this->noiseModel_->WhitenSystem(E, b);
309 for (size_t i = 0; i < Fs.size(); i++) {
310 Fs[i] = this->noiseModel_->Whiten(Fs[i]);
311 }
312
313 const Matrix3 P = Base::Cameras::PointCov(E, lambda, diagonalDamping);
314
315 // Build augmented Hessian (with last row/column being the information
316 // vector) Note: we need to get the augumented hessian wrt the unique keys
317 // in key_
318 SymmetricBlockMatrix augmentedHessianUniqueKeys =
319 Base::Cameras::template SchurComplementAndRearrangeBlocks<3, 6, 6>(
320 Fs, E, P, b, nonUniqueKeys_, this->keys_);
321
322 return boost::make_shared<RegularHessianFactor<DimPose> >(
323 this->keys_, augmentedHessianUniqueKeys);
324 }
325
333 boost::shared_ptr<GaussianFactor> linearizeDamped(
334 const Values& values, const double& lambda = 0.0) const {
335 // depending on flag set on construction we may linearize to different
336 // linear factors
337 switch (this->params_.linearizationMode) {
338 case HESSIAN:
339 return this->createHessianFactor(values, lambda);
340 default:
341 throw std::runtime_error(
342 "SmartProjectionRigFactor: unknown linearization mode");
343 }
344 }
345
347 boost::shared_ptr<GaussianFactor> linearize(
348 const Values& values) const override {
349 return this->linearizeDamped(values);
350 }
351
352 private:
355 template <class ARCHIVE>
356 void serialize(ARCHIVE& ar, const unsigned int /*version*/) {
357 ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base);
358 // ar& BOOST_SERIALIZATION_NVP(nonUniqueKeys_);
359 // ar& BOOST_SERIALIZATION_NVP(cameraRig_);
360 // ar& BOOST_SERIALIZATION_NVP(cameraIds_);
361 }
362};
363// end of class declaration
364
366template <class CAMERA>
368 : public Testable<SmartProjectionRigFactor<CAMERA> > {};
369
370} // namespace gtsam
Smart factor on cameras (pose + calibration)
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
Vector2 Point2
As of GTSAM 4, in order to make GTSAM more lean, it is now possible to just typedef Point2 to Vector2...
Definition: Point2.h:27
noiseModel::Base::shared_ptr SharedNoiseModel
Note, deliberately not in noiseModel namespace.
Definition: NoiseModel.h:736
bool equal(const T &obj1, const T &obj2, double tol)
Call equal on the object.
Definition: Testable.h:84
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
Definition: SymmetricBlockMatrix.h:52
A helper that implements the traits interface for GTSAM types.
Definition: Testable.h:151
A set of cameras, all with their own calibration.
Definition: CameraSet.h:35
Definition: Pose3.h:37
Pose3 inverse() const
inverse transformation with derivatives
Definition: Pose3.cpp:50
This is the base class for all factor types.
Definition: Factor.h:56
KeyVector keys_
The keys involved in this factor.
Definition: Factor.h:73
const_iterator begin() const
Iterator at beginning of involved variable keys.
Definition: Factor.h:128
Nonlinear factor base class.
Definition: NonlinearFactor.h:43
virtual bool active(const Values &) const
Checks whether a factor should be used based on a set of values.
Definition: NonlinearFactor.h:106
A non-templated config holding any types of Manifold-group elements.
Definition: Values.h:63
const ValueType at(Key j) const
Retrieve a variable by key j.
Definition: Values-inl.h:346
SharedIsotropic noiseModel_
As of Feb 22, 2015, the noise model is the same for all measurements and is isotropic.
Definition: SmartFactorBase.h:72
ZVector measured_
Measurements for each of the m views.
Definition: SmartFactorBase.h:79
const ZVector & measured() const
Return the 2D measurements (ZDim, in general).
Definition: SmartFactorBase.h:159
Definition: SmartFactorParams.h:42
LinearizationMode linearizationMode
How to linearize the factor.
Definition: SmartFactorParams.h:44
DegeneracyMode degeneracyMode
How to linearize the factor.
Definition: SmartFactorParams.h:45
SmartProjectionFactor: triangulates point and keeps an estimate of it around.
Definition: SmartProjectionFactor.h:45
void print(const std::string &s="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
print
Definition: SmartProjectionFactor.h:103
TriangulationResult result_
result from triangulateSafe
Definition: SmartProjectionFactor.h:63
TriangulationResult triangulateSafe(const Cameras &cameras) const
Call gtsam::triangulateSafe iff we need to re-triangulate.
Definition: SmartProjectionFactor.h:174
double totalReprojectionError(const Cameras &cameras, boost::optional< Point3 > externalPoint=boost::none) const
Calculate the error of the factor.
Definition: SmartProjectionFactor.h:411
bool equals(const NonlinearFactor &p, double tol=1e-9) const override
equals
Definition: SmartProjectionFactor.h:115
CAMERA Camera
shorthand for a set of cameras
Definition: SmartProjectionFactor.h:74
Definition: SmartProjectionRigFactor.h:52
KeyVector nonUniqueKeys_
vector of keys (one for each observation) with potentially repeated keys
Definition: SmartProjectionRigFactor.h:63
FastVector< size_t > cameraIds_
vector of camera Ids (one for each observation, in the same order), identifying which camera took the...
Definition: SmartProjectionRigFactor.h:70
const FastVector< size_t > & cameraIds() const
return the calibration object
Definition: SmartProjectionRigFactor.h:175
boost::shared_ptr< RegularHessianFactor< DimPose > > createHessianFactor(const Values &values, const double &lambda=0.0, bool diagonalDamping=false) const
linearize and return a Hessianfactor that is an approximation of error(p)
Definition: SmartProjectionRigFactor.h:263
boost::shared_ptr< This > shared_ptr
shorthand for a smart pointer to a factor
Definition: SmartProjectionRigFactor.h:79
void add(const Point2Vector &measurements, const KeyVector &poseKeys, const FastVector< size_t > &cameraIds=FastVector< size_t >())
Variant of the previous "add" function in which we include multiple measurements.
Definition: SmartProjectionRigFactor.h:147
void computeJacobiansWithTriangulatedPoint(typename Base::FBlocks &Fs, Matrix &E, Vector &b, const Cameras &cameras) const
Compute jacobian F, E and error vector at a given linearization point.
Definition: SmartProjectionRigFactor.h:245
SmartProjectionRigFactor()
Default constructor, only for serialization.
Definition: SmartProjectionRigFactor.h:82
double error(const Values &values) const override
error calculates the error of the factor.
Definition: SmartProjectionRigFactor.h:228
Base::Cameras cameras(const Values &values) const override
Collect all cameras involved in this factor.
Definition: SmartProjectionRigFactor.h:210
~SmartProjectionRigFactor() override=default
Virtual destructor.
void add(const Point2 &measured, const Key &poseKey, const size_t &cameraId=0)
add a new measurement, corresponding to an observation from pose "poseKey" and taken from the camera ...
Definition: SmartProjectionRigFactor.h:121
void print(const std::string &s="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
print
Definition: SmartProjectionRigFactor.h:182
boost::shared_ptr< GaussianFactor > linearize(const Values &values) const override
linearize
Definition: SmartProjectionRigFactor.h:347
friend class boost::serialization::access
Serialization function.
Definition: SmartProjectionRigFactor.h:354
const KeyVector & nonUniqueKeys() const
return (for each observation) the (possibly non unique) keys involved in the measurements
Definition: SmartProjectionRigFactor.h:169
boost::shared_ptr< typename Base::Cameras > cameraRig_
cameras in the rig (fixed poses wrt body and intrinsics, for each camera)
Definition: SmartProjectionRigFactor.h:66
SmartProjectionRigFactor(const SharedNoiseModel &sharedNoiseModel, const boost::shared_ptr< Cameras > &cameraRig, const SmartProjectionParams &params=SmartProjectionParams())
Constructor.
Definition: SmartProjectionRigFactor.h:92
const boost::shared_ptr< Cameras > & cameraRig() const
return the calibration object
Definition: SmartProjectionRigFactor.h:172
boost::shared_ptr< GaussianFactor > linearizeDamped(const Values &values, const double &lambda=0.0) const
Linearize to Gaussian Factor (possibly adding a damping factor Lambda for LM)
Definition: SmartProjectionRigFactor.h:333
bool equals(const NonlinearFactor &p, double tol=1e-9) const override
equals
Definition: SmartProjectionRigFactor.h:196