From 319d26312e779b3dbcb6b83b489e251f91b76d34 Mon Sep 17 00:00:00 2001 From: dellaert Date: Thu, 19 Feb 2015 02:01:36 +0100 Subject: [PATCH] Better documentation --- gtsam/slam/SmartFactorBase.h | 116 +++++++++++++++++++++-------------- 1 file changed, 71 insertions(+), 45 deletions(-) diff --git a/gtsam/slam/SmartFactorBase.h b/gtsam/slam/SmartFactorBase.h index 88afe2c21..f9f1c6e2b 100644 --- a/gtsam/slam/SmartFactorBase.h +++ b/gtsam/slam/SmartFactorBase.h @@ -40,6 +40,9 @@ namespace gtsam { * @brief Base class for smart factors * This base class has no internal point, but it has a measurement, noise model * and an optional sensor pose. + * This class mainly computes the derivatives and returns them as a variety of factors. + * The methods take a Cameras argument, which should behave like PinholeCamera, and + * the value of a point, which is kept in the base class. */ template class SmartFactorBase: public NonlinearFactor { @@ -47,9 +50,12 @@ class SmartFactorBase: public NonlinearFactor { protected: // Keep a copy of measurement for I/O - std::vector measured_; ///< 2D measurement for each of the m views - ///< (important that the order is the same as the keys that we use to create the factor) + /** + * 2D measurement and noise model for each of the m views + * The order is kept the same as the keys that we use to create the factor. + */ + std::vector measured_; std::vector noise_; ///< noise model used boost::optional body_P_sensor_; ///< The pose of the sensor in the body frame (one for all cameras) @@ -95,7 +101,7 @@ public: } /** - * add a new measurement and pose key + * Add a new measurement and pose key * @param measured_i is the 2m dimensional projection of a single landmark * @param poseKey is the index corresponding to the camera observing the landmark * @param noise_i is the measurement noise @@ -108,7 +114,7 @@ public: } /** - * variant of the previous add: adds a bunch of measurements, together with the camera keys and noises + * Add a bunch of measurements, together with the camera keys and noises */ void add(std::vector& measurements, std::vector& poseKeys, std::vector& noises) { @@ -120,7 +126,7 @@ public: } /** - * variant of the previous add: adds a bunch of measurements and uses the same noise model for all of them + * Add a bunch of measurements and uses the same noise model for all of them */ void add(std::vector& measurements, std::vector& poseKeys, const SharedNoiseModel& noise) { @@ -149,7 +155,7 @@ public: return measured_; } - /** return the noise model */ + /** return the noise models */ const std::vector& noise() const { return noise_; } @@ -237,7 +243,11 @@ public: return overallError; } - /// Assumes non-degenerate ! + /** + * Compute the matrices E and PointCov = inv(E'*E), where E stacks the ZDim*3 derivatives + * of project with respect to the point, given each of the m cameras. + * Assumes non-degenerate ! + */ void computeEP(Matrix& E, Matrix& PointCov, const Cameras& cameras, const Point3& point) const { @@ -262,8 +272,12 @@ public: } /** - * Compute F, E only (called below in both vanilla and SVD versions) - * Given a Point3, assumes dimensionality is 3 + * Compute F, E, and b (called below in both vanilla and SVD versions), where + * F is a vector of derivatives wrpt the cameras, and E the stacked derivatives + * with respect to the point. The value of cameras/point are passed as parameters. + * Given a Point3, assumes dimensionality is 3. + * TODO We assume below the dimensionality of POSE is 6. Frank thinks the templating + * of this factor is only for show, and should just assume a PinholeCamera. */ double computeJacobians(std::vector& Fblocks, Matrix& E, Vector& b, const Cameras& cameras, const Point3& point) const { @@ -273,13 +287,15 @@ public: b = zero(ZDim * numKeys); double f = 0; - Matrix Fi(ZDim, 6), Ei(ZDim, 3), Hcali(ZDim, D - 6), Hcam(ZDim, D); + Matrix Fi(ZDim, 6), Ei(ZDim, 3), Hcali(ZDim, D - 6); for (size_t i = 0; i < this->measured_.size(); i++) { Vector bi; try { - bi = - -(cameras[i].project(point, Fi, Ei, Hcali) - this->measured_.at(i)).vector(); + const Z& zi = this->measured_.at(i); + bi = -(cameras[i].project(point, Fi, Ei, Hcali) - zi).vector(); + + // if we have a sensor offset, correct derivatives if (body_P_sensor_) { Pose3 w_Pose_body = (cameras[i].pose()).compose( body_P_sensor_->inverse()); @@ -289,14 +305,18 @@ public: } } catch (CheiralityException&) { std::cout << "Cheirality exception " << std::endl; - exit(EXIT_FAILURE); + exit(EXIT_FAILURE); // TODO: throw exception } + + // Whiten derivatives according to noise parameters this->noise_.at(i)->WhitenSystem(Fi, Ei, Hcali, bi); f += bi.squaredNorm(); - if (D == 6) { // optimize only camera pose + // TODO: if D==6 we optimize only camera pose. What's up with that ??? + if (D == 6) { Fblocks.push_back(KeyMatrix2D(this->keys_[i], Fi)); } else { + Matrix Hcam(ZDim, D); Hcam.block(0, 0) = Fi; // ZDim x 6 block for the cameras Hcam.block(0, 6) = Hcali; // ZDim x nrCal block for the cameras Fblocks.push_back(KeyMatrix2D(this->keys_[i], Hcam)); @@ -386,7 +406,9 @@ public: return f; } - /// linearize returns a Hessianfactor that is an approximation of error(p) + /** + * Linearize returns a Hessianfactor that is an approximation of error(p) + */ boost::shared_ptr > createHessianFactor( const Cameras& cameras, const Point3& point, const double lambda = 0.0, bool diagonalDamping = false) const { @@ -412,8 +434,7 @@ public: //std::vector < Matrix > Gs2(Gs.begin(), Gs.end()); //std::vector < Vector > gs2(gs.begin(), gs.end()); - return boost::make_shared < RegularHessianFactor - > (this->keys_, Gs, gs, f); + return boost::make_shared < RegularHessianFactor > (this->keys_, Gs, gs, f); #else // we create directly a SymmetricBlockMatrix size_t n1 = D * numKeys + 1; std::vector dims(numKeys + 1); // this also includes the b term @@ -429,7 +450,8 @@ public: } /** - * slow version - works on full (sparse) matrices + * Do Schur complement, given Jacobian as F,E,PointCov. + * Slow version - works on full matrices */ void schurComplement(const std::vector& Fblocks, const Matrix& E, const Matrix& PointCov, const Vector& b, @@ -467,7 +489,10 @@ public: } } - // **************************************************************************************************** + /** + * Do Schur complement, given Jacobian as F,E,PointCov, return SymmetricBlockMatrix + * Fast version - works on with sparsity + */ void sparseSchurComplement(const std::vector& Fblocks, const Matrix& E, const Matrix& P /*Point Covariance*/, const Vector& b, /*output ->*/SymmetricBlockMatrix& augmentedHessian) const { @@ -506,7 +531,8 @@ public: } /** - * + * Do Schur complement, given Jacobian as F,E,PointCov, return Gs/gs + * Fast version - works on with sparsity */ void sparseSchurComplement(const std::vector& Fblocks, const Matrix& E, const Matrix& P /*Point Covariance*/, const Vector& b, @@ -554,28 +580,7 @@ public: } /** - * - */ - void updateAugmentedHessian(const Cameras& cameras, const Point3& point, - const double lambda, bool diagonalDamping, - SymmetricBlockMatrix& augmentedHessian, - const FastVector allKeys) const { - - // int numKeys = this->keys_.size(); - - std::vector Fblocks; - Matrix E; - Matrix3 PointCov; - Vector b; - double f = computeJacobians(Fblocks, E, PointCov, b, cameras, point, lambda, - diagonalDamping); - - updateSparseSchurComplement(Fblocks, E, PointCov, b, f, allKeys, - augmentedHessian); // augmentedHessian.matrix().block (i1,i2) = ... - } - - /** - * + * No idea. TODO Luca should document */ void updateSparseSchurComplement(const std::vector& Fblocks, const Matrix& E, const Matrix& P /*Point Covariance*/, const Vector& b, @@ -648,7 +653,28 @@ public: } /** - * + * No idea. TODO Luca should document + */ + void updateAugmentedHessian(const Cameras& cameras, const Point3& point, + const double lambda, bool diagonalDamping, + SymmetricBlockMatrix& augmentedHessian, + const FastVector allKeys) const { + + // int numKeys = this->keys_.size(); + + std::vector Fblocks; + Matrix E; + Matrix3 PointCov; + Vector b; + double f = computeJacobians(Fblocks, E, PointCov, b, cameras, point, lambda, + diagonalDamping); + + updateSparseSchurComplement(Fblocks, E, PointCov, b, f, allKeys, + augmentedHessian); // augmentedHessian.matrix().block (i1,i2) = ... + } + + /** + * Return Jacobians as RegularImplicitSchurFactor with raw access */ boost::shared_ptr > createRegularImplicitSchurFactor( const Cameras& cameras, const Point3& point, double lambda = 0.0, @@ -662,7 +688,7 @@ public: } /** - * + * Return Jacobians as JacobianFactorQ */ boost::shared_ptr > createJacobianQFactor( const Cameras& cameras, const Point3& point, double lambda = 0.0, @@ -678,7 +704,7 @@ public: } /** - * + * Return Jacobians as JacobianFactor */ boost::shared_ptr createJacobianSVDFactor( const Cameras& cameras, const Point3& point, double lambda = 0.0) const {