gtsam 4.2.0
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 typedef typename CAMERA::Measurement MEASUREMENT;
58 typedef typename CAMERA::MeasurementVector MEASUREMENTS;
59
60 static const int DimPose = 6;
61 static const int ZDim = 2;
62
63 protected:
66
68 boost::shared_ptr<typename Base::Cameras> cameraRig_;
69
73
74 public:
75 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
76
77 typedef CAMERA Camera;
79
81 typedef boost::shared_ptr<This> shared_ptr;
82
85
95 const SharedNoiseModel& sharedNoiseModel,
96 const boost::shared_ptr<Cameras>& cameraRig,
98 : Base(sharedNoiseModel, params), cameraRig_(cameraRig) {
99 // throw exception if configuration is not supported by this factor
100 if (Base::params_.degeneracyMode != gtsam::ZERO_ON_DEGENERACY)
101 throw std::runtime_error(
102 "SmartProjectionRigFactor: "
103 "degeneracyMode must be set to ZERO_ON_DEGENERACY");
104 if (Base::params_.linearizationMode != gtsam::HESSIAN)
105 throw std::runtime_error(
106 "SmartProjectionRigFactor: "
107 "linearizationMode must be set to HESSIAN");
108 }
109
111 ~SmartProjectionRigFactor() override = default;
112
123 void add(const MEASUREMENT& measured, const Key& poseKey,
124 const size_t& cameraId = 0) {
125 // store measurement and key
126 this->measured_.push_back(measured);
127 this->nonUniqueKeys_.push_back(poseKey);
128
129 // also store keys in the keys_ vector: these keys are assumed to be
130 // unique, so we avoid duplicates here
131 if (std::find(this->keys_.begin(), this->keys_.end(), poseKey) ==
132 this->keys_.end())
133 this->keys_.push_back(poseKey); // add only unique keys
134
135 // store id of the camera taking the measurement
136 cameraIds_.push_back(cameraId);
137 }
138
149 void add(const MEASUREMENTS& measurements, const KeyVector& poseKeys,
151 if (poseKeys.size() != measurements.size() ||
152 (poseKeys.size() != cameraIds.size() && cameraIds.size() != 0)) {
153 throw std::runtime_error(
154 "SmartProjectionRigFactor: "
155 "trying to add inconsistent inputs");
156 }
157 if (cameraIds.size() == 0 && cameraRig_->size() > 1) {
158 throw std::runtime_error(
159 "SmartProjectionRigFactor: "
160 "camera rig includes multiple camera "
161 "but add did not input cameraIds");
162 }
163 for (size_t i = 0; i < measurements.size(); i++) {
164 add(measurements[i], poseKeys[i],
165 cameraIds.size() == 0 ? 0 : cameraIds[i]);
166 }
167 }
168
171 const KeyVector& nonUniqueKeys() const { return nonUniqueKeys_; }
172
174 const boost::shared_ptr<Cameras>& cameraRig() const { return cameraRig_; }
175
177 const FastVector<size_t>& cameraIds() const { return cameraIds_; }
178
184 void print(
185 const std::string& s = "",
186 const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override {
187 std::cout << s << "SmartProjectionRigFactor: \n ";
188 for (size_t i = 0; i < nonUniqueKeys_.size(); i++) {
189 std::cout << "-- Measurement nr " << i << std::endl;
190 std::cout << "key: " << keyFormatter(nonUniqueKeys_[i]) << std::endl;
191 std::cout << "cameraId: " << cameraIds_[i] << std::endl;
192 (*cameraRig_)[cameraIds_[i]].print("camera in rig:\n");
193 }
194 Base::print("", keyFormatter);
195 }
196
198 bool equals(const NonlinearFactor& p, double tol = 1e-9) const override {
199 const This* e = dynamic_cast<const This*>(&p);
200 return e && Base::equals(p, tol) && nonUniqueKeys_ == e->nonUniqueKeys() &&
201 cameraRig_->equals(*(e->cameraRig())) &&
202 std::equal(cameraIds_.begin(), cameraIds_.end(),
203 e->cameraIds().begin());
204 }
205
212 typename Base::Cameras cameras(const Values& values) const override {
213 typename Base::Cameras cameras;
214 cameras.reserve(nonUniqueKeys_.size()); // preallocate
215 for (size_t i = 0; i < nonUniqueKeys_.size(); i++) {
216 const typename Base::Camera& camera_i = (*cameraRig_)[cameraIds_[i]];
217 const Pose3 world_P_sensor_i =
218 values.at<Pose3>(nonUniqueKeys_[i]) // = world_P_body
219 * camera_i.pose(); // = body_P_cam_i
220 cameras.emplace_back(world_P_sensor_i,
221 make_shared<typename CAMERA::CalibrationType>(
222 camera_i.calibration()));
223 }
224 return cameras;
225 }
226
230 double error(const Values& values) const override {
231 if (this->active(values)) {
232 return this->totalReprojectionError(this->cameras(values));
233 } else { // else of active flag
234 return 0.0;
235 }
236 }
237
247 void computeJacobiansWithTriangulatedPoint(typename Base::FBlocks& Fs,
248 Matrix& E, Vector& b,
249 const Cameras& cameras) const {
250 if (!this->result_) {
251 throw("computeJacobiansWithTriangulatedPoint");
252 } else { // valid result: compute jacobians
253 b = -cameras.reprojectionError(*this->result_, this->measured_, Fs, E);
254 for (size_t i = 0; i < Fs.size(); i++) {
255 const Pose3& body_P_sensor = (*cameraRig_)[cameraIds_[i]].pose();
256 const Pose3 world_P_body = cameras[i].pose() * body_P_sensor.inverse();
257 Eigen::Matrix<double, DimPose, DimPose> H;
258 world_P_body.compose(body_P_sensor, H);
259 Fs.at(i) = Fs.at(i) * H;
260 }
261 }
262 }
263
265 boost::shared_ptr<RegularHessianFactor<DimPose> > createHessianFactor(
266 const Values& values, const double& lambda = 0.0,
267 bool diagonalDamping = false) const {
268 // we may have multiple observation sharing the same keys (e.g., 2 cameras
269 // measuring from the same body pose), hence the number of unique keys may
270 // be smaller than nrMeasurements
271 size_t nrUniqueKeys =
272 this->keys_
273 .size(); // note: by construction, keys_ only contains unique keys
274
275 Cameras cameras = this->cameras(values);
276
277 // Create structures for Hessian Factors
278 std::vector<size_t> js;
279 std::vector<Matrix> Gs(nrUniqueKeys * (nrUniqueKeys + 1) / 2);
280 std::vector<Vector> gs(nrUniqueKeys);
281
282 if (this->measured_.size() != cameras.size()) // 1 observation per camera
283 throw std::runtime_error(
284 "SmartProjectionRigFactor: "
285 "measured_.size() inconsistent with input");
286
287 // triangulate 3D point at given linearization point
288 this->triangulateSafe(cameras);
289
290 if (!this->result_) { // failed: return "empty/zero" Hessian
291 if (this->params_.degeneracyMode == ZERO_ON_DEGENERACY) {
292 for (Matrix& m : Gs) m = Matrix::Zero(DimPose, DimPose);
293 for (Vector& v : gs) v = Vector::Zero(DimPose);
294 return boost::make_shared<RegularHessianFactor<DimPose> >(this->keys_,
295 Gs, gs, 0.0);
296 } else {
297 throw std::runtime_error(
298 "SmartProjectionRigFactor: "
299 "only supported degeneracy mode is ZERO_ON_DEGENERACY");
300 }
301 }
302
303 // compute Jacobian given triangulated 3D Point
304 typename Base::FBlocks Fs;
305 Matrix E;
306 Vector b;
308
309 // Whiten using noise model
310 this->noiseModel_->WhitenSystem(E, b);
311 for (size_t i = 0; i < Fs.size(); i++) {
312 Fs[i] = this->noiseModel_->Whiten(Fs[i]);
313 }
314
315 const Matrix3 P = Base::Cameras::PointCov(E, lambda, diagonalDamping);
316
317 // Build augmented Hessian (with last row/column being the information
318 // vector) Note: we need to get the augumented hessian wrt the unique keys
319 // in key_
320 SymmetricBlockMatrix augmentedHessianUniqueKeys =
321 Base::Cameras::template SchurComplementAndRearrangeBlocks<3, 6, 6>(
322 Fs, E, P, b, nonUniqueKeys_, this->keys_);
323
324 return boost::make_shared<RegularHessianFactor<DimPose> >(
325 this->keys_, augmentedHessianUniqueKeys);
326 }
327
335 boost::shared_ptr<GaussianFactor> linearizeDamped(
336 const Values& values, const double& lambda = 0.0) const {
337 // depending on flag set on construction we may linearize to different
338 // linear factors
339 switch (this->params_.linearizationMode) {
340 case HESSIAN:
341 return this->createHessianFactor(values, lambda);
342 default:
343 throw std::runtime_error(
344 "SmartProjectionRigFactor: unknown linearization mode");
345 }
346 }
347
349 boost::shared_ptr<GaussianFactor> linearize(
350 const Values& values) const override {
351 return this->linearizeDamped(values);
352 }
353
354 private:
357 template <class ARCHIVE>
358 void serialize(ARCHIVE& ar, const unsigned int /*version*/) {
359 ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base);
360 // ar& BOOST_SERIALIZATION_NVP(nonUniqueKeys_);
361 // ar& BOOST_SERIALIZATION_NVP(cameraRig_);
362 // ar& BOOST_SERIALIZATION_NVP(cameraIds_);
363 }
364};
365// end of class declaration
366
368template <class CAMERA>
370 : public Testable<SmartProjectionRigFactor<CAMERA> > {};
371
372} // namespace gtsam
Smart factor on cameras (pose + calibration)
std::vector< T, typename internal::FastDefaultVectorAllocator< T >::type > FastVector
FastVector is a type alias to a std::vector with a custom memory allocator.
Definition: FastVector.h:34
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
noiseModel::Base::shared_ptr SharedNoiseModel
Aliases.
Definition: NoiseModel.h:724
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:100
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
This class stores a dense matrix and allows it to be accessed as a collection of blocks.
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:36
A 3D pose (R,t) : (Rot3,Point3)
Definition: Pose3.h:37
Pose3 inverse() const
inverse transformation with derivatives
Definition: Pose3.cpp:49
Definition: Factor.h:68
KeyVector keys_
The keys involved in this factor.
Definition: Factor.h:85
const_iterator begin() const
Iterator at beginning of involved variable keys.
Definition: Factor.h:143
Nonlinear factor base class.
Definition: NonlinearFactor.h:42
virtual bool active(const Values &) const
Checks whether a factor should be used based on a set of values.
Definition: NonlinearFactor.h:118
A non-templated config holding any types of Manifold-group elements.
Definition: Values.h:65
const ValueType at(Key j) const
Retrieve a variable by key j.
Definition: Values-inl.h:361
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
If you are using the factor, please cite: L.
Definition: SmartProjectionRigFactor.h:52
KeyVector nonUniqueKeys_
vector of keys (one for each observation) with potentially repeated keys
Definition: SmartProjectionRigFactor.h:65
FastVector< size_t > cameraIds_
vector of camera Ids (one for each observation, in the same order), identifying which camera took the...
Definition: SmartProjectionRigFactor.h:72
const FastVector< size_t > & cameraIds() const
return the calibration object
Definition: SmartProjectionRigFactor.h:177
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:265
void add(const MEASUREMENT &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:123
boost::shared_ptr< This > shared_ptr
shorthand for a smart pointer to a factor
Definition: SmartProjectionRigFactor.h:81
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:247
void add(const MEASUREMENTS &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:149
SmartProjectionRigFactor()
Default constructor, only for serialization.
Definition: SmartProjectionRigFactor.h:84
double error(const Values &values) const override
error calculates the error of the factor.
Definition: SmartProjectionRigFactor.h:230
Base::Cameras cameras(const Values &values) const override
Collect all cameras involved in this factor.
Definition: SmartProjectionRigFactor.h:212
~SmartProjectionRigFactor() override=default
Virtual destructor.
void print(const std::string &s="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
print
Definition: SmartProjectionRigFactor.h:184
boost::shared_ptr< GaussianFactor > linearize(const Values &values) const override
linearize
Definition: SmartProjectionRigFactor.h:349
friend class boost::serialization::access
Serialization function.
Definition: SmartProjectionRigFactor.h:356
const KeyVector & nonUniqueKeys() const
return (for each observation) the (possibly non unique) keys involved in the measurements
Definition: SmartProjectionRigFactor.h:171
boost::shared_ptr< typename Base::Cameras > cameraRig_
cameras in the rig (fixed poses wrt body and intrinsics, for each camera)
Definition: SmartProjectionRigFactor.h:68
SmartProjectionRigFactor(const SharedNoiseModel &sharedNoiseModel, const boost::shared_ptr< Cameras > &cameraRig, const SmartProjectionParams &params=SmartProjectionParams())
Constructor.
Definition: SmartProjectionRigFactor.h:94
const boost::shared_ptr< Cameras > & cameraRig() const
return the calibration object
Definition: SmartProjectionRigFactor.h:174
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:335
bool equals(const NonlinearFactor &p, double tol=1e-9) const override
equals
Definition: SmartProjectionRigFactor.h:198