diff --git a/gtsam/slam/JacobianFactorQ.h b/gtsam/slam/JacobianFactorQ.h index 5e8693541..5dd7582cd 100644 --- a/gtsam/slam/JacobianFactorQ.h +++ b/gtsam/slam/JacobianFactorQ.h @@ -28,7 +28,7 @@ class JacobianFactorQ: public RegularJacobianFactor { typedef RegularJacobianFactor Base; typedef Eigen::Matrix MatrixZD; - typedef std::pair KeyMatrixZD; + typedef std::pair KeyMatrix; public: @@ -42,7 +42,6 @@ public: Base() { Matrix zeroMatrix = Matrix::Zero(0, D); Vector zeroVector = Vector::Zero(0); - typedef std::pair KeyMatrix; std::vector QF; QF.reserve(keys.size()); BOOST_FOREACH(const Key& key, keys) @@ -51,22 +50,23 @@ public: } /// Constructor - JacobianFactorQ(const std::vector& Fblocks, const Matrix& E, - const Matrix3& P, const Vector& b, const SharedDiagonal& model = - SharedDiagonal()) : + JacobianFactorQ(const FastVector& keys, + const std::vector& FBlocks, const Matrix& E, const Matrix3& P, + const Vector& b, const SharedDiagonal& model = SharedDiagonal()) : Base() { size_t j = 0, m2 = E.rows(), m = m2 / ZDim; // Calculate projector Q Matrix Q = eye(m2) - E * P * E.transpose(); // Calculate pre-computed Jacobian matrices // TODO: can we do better ? - typedef std::pair KeyMatrix; std::vector QF; QF.reserve(m); // Below, we compute each mZDim*D block A_j = Q_j * F_j = (mZDim*ZDim) * (Zdim*D) - BOOST_FOREACH(const KeyMatrixZD& it, Fblocks) + for (size_t k = 0; k < FBlocks.size(); ++k) { + Key key = keys[k]; QF.push_back( - KeyMatrix(it.first, Q.block(0, ZDim * j++, m2, ZDim) * it.second)); + KeyMatrix(key, Q.block(0, ZDim * j++, m2, ZDim) * FBlocks[k])); + } // Which is then passed to the normal JacobianFactor constructor JacobianFactor::fillTerms(QF, Q * b, model); } diff --git a/gtsam/slam/JacobianFactorQR.h b/gtsam/slam/JacobianFactorQR.h index 9c3f8be4a..77102c24b 100644 --- a/gtsam/slam/JacobianFactorQR.h +++ b/gtsam/slam/JacobianFactorQR.h @@ -7,8 +7,8 @@ #pragma once #include -#include #include +#include namespace gtsam { @@ -22,25 +22,24 @@ class JacobianFactorQR: public RegularJacobianFactor { typedef RegularJacobianFactor Base; typedef Eigen::Matrix MatrixZD; - typedef std::pair KeyMatrixZD; public: /** * Constructor */ - JacobianFactorQR(const std::vector& Fblocks, const Matrix& E, - const Matrix3& P, const Vector& b, // + JacobianFactorQR(const FastVector& keys, + const std::vector& FBlocks, const Matrix& E, const Matrix3& P, + const Vector& b, // const SharedDiagonal& model = SharedDiagonal()) : Base() { // Create a number of Jacobian factors in a factor graph GaussianFactorGraph gfg; Symbol pointKey('p', 0); - size_t i = 0; - BOOST_FOREACH(const KeyMatrixZD& it, Fblocks) { - gfg.add(pointKey, E.block(ZDim * i, 0), it.first, it.second, - b.segment(ZDim * i), model); - i += 1; + for (size_t k = 0; k < FBlocks.size(); ++k) { + Key key = keys[k]; + gfg.add(pointKey, E.block(ZDim * k, 0), key, FBlocks[k], + b.segment < ZDim > (ZDim * k), model); } //gfg.print("gfg"); diff --git a/gtsam/slam/RegularImplicitSchurFactor.h b/gtsam/slam/RegularImplicitSchurFactor.h index fd1a5764d..dac5ad153 100644 --- a/gtsam/slam/RegularImplicitSchurFactor.h +++ b/gtsam/slam/RegularImplicitSchurFactor.h @@ -7,9 +7,9 @@ #pragma once +#include #include #include -#include #include #include @@ -18,7 +18,7 @@ namespace gtsam { /** * RegularImplicitSchurFactor */ -template // +template class RegularImplicitSchurFactor: public GaussianFactor { public: @@ -27,22 +27,21 @@ public: protected: - typedef Eigen::Matrix Matrix2D; ///< type of an F block - typedef Eigen::Matrix MatrixDD; ///< camera hessian - typedef std::pair KeyMatrix2D; ///< named F block + // This factor is closely related to a CameraSet + typedef CameraSet Set; - const std::vector Fblocks_; ///< All Z*D F blocks (one for each camera) - const Matrix3 PointCovariance_; ///< the 3*3 matrix P = inv(E'E) (Z*Z if degenerate) + typedef typename CAMERA::Measurement Z; + static const int D = traits::dimension; ///< Camera dimension + static const int ZDim = traits::dimension; ///< Measurement dimension + + typedef Eigen::Matrix MatrixZD; ///< type of an F block + typedef Eigen::Matrix MatrixDD; ///< camera hessian + + const std::vector FBlocks_; ///< All ZDim*D F blocks (one for each camera) + const Matrix3 PointCovariance_; ///< the 3*3 matrix P = inv(E'E) (ZDim*ZDim if degenerate) const Matrix E_; ///< The 2m*3 E Jacobian with respect to the point const Vector b_; ///< 2m-dimensional RHS vector - /// initialize keys from Fblocks - void initKeys() { - keys_.reserve(Fblocks_.size()); - BOOST_FOREACH(const KeyMatrix2D& it, Fblocks_) - keys_.push_back(it.first); - } - public: /// Constructor @@ -50,29 +49,29 @@ public: } /// Construct from blocks of F, E, inv(E'*E), and RHS vector b - RegularImplicitSchurFactor(const std::vector& Fblocks, - const Matrix& E, const Matrix3& P, const Vector& b) : - Fblocks_(Fblocks), PointCovariance_(P), E_(E), b_(b) { - initKeys(); + RegularImplicitSchurFactor(const FastVector& keys, + const std::vector& FBlocks, const Matrix& E, const Matrix3& P, + const Vector& b) : + GaussianFactor(keys), FBlocks_(FBlocks), PointCovariance_(P), E_(E), b_(b) { } /// Destructor virtual ~RegularImplicitSchurFactor() { } - inline std::vector& Fblocks() const { - return Fblocks_; + std::vector& FBlocks() const { + return FBlocks_; } - inline const Matrix& E() const { + const Matrix& E() const { return E_; } - inline const Vector& b() const { + const Vector& b() const { return b_; } - inline const Matrix3& getPointCovariance() const { + const Matrix3& getPointCovariance() const { return PointCovariance_; } @@ -82,7 +81,7 @@ public: std::cout << " RegularImplicitSchurFactor " << std::endl; Factor::print(s); for (size_t pos = 0; pos < size(); ++pos) { - std::cout << "Fblock:\n" << Fblocks_[pos].second << std::endl; + std::cout << "Fblock:\n" << FBlocks_[pos] << std::endl; } std::cout << "PointCovariance:\n" << PointCovariance_ << std::endl; std::cout << "E:\n" << E_ << std::endl; @@ -94,13 +93,12 @@ public: const This* f = dynamic_cast(&lf); if (!f) return false; - for (size_t pos = 0; pos < size(); ++pos) { - if (keys_[pos] != f->keys_[pos]) + for (size_t k = 0; k < FBlocks_.size(); ++k) { + if (keys_[k] != f->keys_[k]) return false; - if (Fblocks_[pos].first != f->Fblocks_[pos].first) + if (FBlocks_[k] != f->FBlocks_[k]) return false; - if (!equal_with_abs_tol(Fblocks_[pos].second, f->Fblocks_[pos].second, - tol)) + if (!equal_with_abs_tol(FBlocks_[k], f->FBlocks_[k], tol)) return false; } return equal_with_abs_tol(PointCovariance_, f->PointCovariance_, tol) @@ -124,131 +122,12 @@ public: return std::make_pair(Matrix(), Vector()); } - /** - * Do Schur complement, given Jacobian as F,E,P, return SymmetricBlockMatrix - * Fast version - works on with sparsity - */ - static void SparseSchurComplement(const std::vector& Fblocks, - const Matrix& E, const Matrix3& P /*Point Covariance*/, const Vector& b, - /*output ->*/SymmetricBlockMatrix& augmentedHessian) { - // Schur complement trick - // G = F' * F - F' * E * P * E' * F - // g = F' * (b - E * P * E' * b) - - // a single point is observed in m cameras - size_t m = Fblocks.size(); - - // Blockwise Schur complement - for (size_t i = 0; i < m; i++) { // for each camera - - const Matrix2D& Fi = Fblocks.at(i).second; - const Matrix23 Ei_P = E.block(Z * i, 0) * P; - - // D = (Dx2) * (Z) - augmentedHessian(i, m) = Fi.transpose() * b.segment(Z * i) // F' * b - - Fi.transpose() * (Ei_P * (E.transpose() * b)); // D = (DxZDim) * (ZDimx3) * (3*ZDimm) * (ZDimm x 1) - - // (DxD) = (DxZDim) * ( (ZDimxD) - (ZDimx3) * (3xZDim) * (ZDimxD) ) - augmentedHessian(i, i) = Fi.transpose() - * (Fi - Ei_P * E.block(Z * i, 0).transpose() * Fi); - - // upper triangular part of the hessian - for (size_t j = i + 1; j < m; j++) { // for each camera - const Matrix2D& Fj = Fblocks.at(j).second; - - // (DxD) = (Dx2) * ( (2x2) * (2xD) ) - augmentedHessian(i, j) = -Fi.transpose() - * (Ei_P * E.block(Z * j, 0).transpose() * Fj); - } - } // end of for over cameras - } - - /** - * Applies Schur complement (exploiting block structure) to get a smart factor on cameras, - * and adds the contribution of the smart factor to a pre-allocated augmented Hessian. - */ - static void UpdateSparseSchurComplement( - const std::vector& Fblocks, const Matrix& E, - const Matrix3& P /*Point Covariance*/, const Vector& b, const double f, - const FastVector& allKeys, const FastVector& keys, - /*output ->*/SymmetricBlockMatrix& augmentedHessian) { - - FastMap KeySlotMap; - for (size_t slot = 0; slot < allKeys.size(); slot++) - KeySlotMap.insert(std::make_pair(allKeys[slot], slot)); - // Schur complement trick - // G = F' * F - F' * E * P * E' * F - // g = F' * (b - E * P * E' * b) - - MatrixDD matrixBlock; - typedef SymmetricBlockMatrix::Block Block; ///< A block from the Hessian matrix - - // a single point is observed in m cameras - size_t m = Fblocks.size(); // cameras observing current point - size_t aug_m = (augmentedHessian.rows() - 1) / D; // all cameras in the group - - // Blockwise Schur complement - for (size_t i = 0; i < m; i++) { // for each camera in the current factor - - const Matrix2D& Fi = Fblocks.at(i).second; - const Matrix23 Ei_P = E.block(Z * i, 0) * P; - - // D = (DxZDim) * (Z) - // allKeys are the list of all camera keys in the group, e.g, (1,3,4,5,7) - // we should map those to a slot in the local (grouped) hessian (0,1,2,3,4) - // Key cameraKey_i = this->keys_[i]; - DenseIndex aug_i = KeySlotMap.at(keys[i]); - - // information vector - store previous vector - // vectorBlock = augmentedHessian(aug_i, aug_m).knownOffDiagonal(); - // add contribution of current factor - augmentedHessian(aug_i, aug_m) = - augmentedHessian(aug_i, aug_m).knownOffDiagonal() - + Fi.transpose() * b.segment(Z * i) // F' * b - - Fi.transpose() * (Ei_P * (E.transpose() * b)); // D = (DxZDim) * (ZDimx3) * (3*ZDimm) * (ZDimm x 1) - - // (DxD) = (DxZDim) * ( (ZDimxD) - (ZDimx3) * (3xZDim) * (ZDimxD) ) - // main block diagonal - store previous block - matrixBlock = augmentedHessian(aug_i, aug_i); - // add contribution of current factor - augmentedHessian(aug_i, aug_i) = matrixBlock - + (Fi.transpose() - * (Fi - Ei_P * E.block(Z * i, 0).transpose() * Fi)); - - // upper triangular part of the hessian - for (size_t j = i + 1; j < m; j++) { // for each camera - const Matrix2D& Fj = Fblocks.at(j).second; - - //Key cameraKey_j = this->keys_[j]; - DenseIndex aug_j = KeySlotMap.at(keys[j]); - - // (DxD) = (DxZDim) * ( (ZDimxZDim) * (ZDimxD) ) - // off diagonal block - store previous block - // matrixBlock = augmentedHessian(aug_i, aug_j).knownOffDiagonal(); - // add contribution of current factor - augmentedHessian(aug_i, aug_j) = - augmentedHessian(aug_i, aug_j).knownOffDiagonal() - - Fi.transpose() - * (Ei_P * E.block(Z * j, 0).transpose() * Fj); - } - } // end of for over cameras - - augmentedHessian(aug_m, aug_m)(0, 0) += f; - } - /// *Compute* full augmented information matrix virtual Matrix augmentedInformation() const { - // Create a SymmetricBlockMatrix - int m = this->keys_.size(); - size_t M1 = D * m + 1; - std::vector dims(m + 1); // this also includes the b term - std::fill(dims.begin(), dims.end() - 1, D); - dims.back() = 1; - SymmetricBlockMatrix augmentedHessian(dims, Matrix::Zero(M1, M1)); - // Do the Schur complement - SparseSchurComplement(Fblocks_, E_, PointCovariance_, b_, augmentedHessian); + SymmetricBlockMatrix augmentedHessian = Set::SchurComplement(FBlocks_, E_, + PointCovariance_, b_); return augmentedHessian.matrix(); } @@ -265,14 +144,14 @@ public: // diag(Hessian) = diag(F' * (I - E * PointCov * E') * F); VectorValues d; - for (size_t pos = 0; pos < size(); ++pos) { // for each camera - Key j = keys_[pos]; + for (size_t k = 0; k < size(); ++k) { // for each camera + Key j = keys_[k]; // Calculate Fj'*Ej for the current camera (observing a single point) - // D x 3 = (D x Z) * (Z x 3) - const Matrix2D& Fj = Fblocks_[pos].second; + // D x 3 = (D x ZDim) * (ZDim x 3) + const MatrixZD& Fj = FBlocks_[k]; Eigen::Matrix FtE = Fj.transpose() - * E_.block(Z * pos, 0); + * E_.block(ZDim * k, 0); Eigen::Matrix dj; for (size_t k = 0; k < D; ++k) { // for each diagonal element of the camera hessian @@ -301,10 +180,10 @@ public: Key j = keys_[pos]; // Calculate Fj'*Ej for the current camera (observing a single point) - // D x 3 = (D x Z) * (Z x 3) - const Matrix2D& Fj = Fblocks_[pos].second; + // D x 3 = (D x ZDim) * (ZDim x 3) + const MatrixZD& Fj = FBlocks_[pos]; Eigen::Matrix FtE = Fj.transpose() - * E_.block(Z * pos, 0); + * E_.block(ZDim * pos, 0); DVector dj; for (size_t k = 0; k < D; ++k) { // for each diagonal element of the camera hessian @@ -323,28 +202,28 @@ public: for (size_t pos = 0; pos < size(); ++pos) { Key j = keys_[pos]; // F'*F - F'*E*P*E'*F e.g. (9*2)*(2*9) - (9*2)*(2*3)*(3*3)*(3*2)*(2*9) - const Matrix2D& Fj = Fblocks_[pos].second; + const MatrixZD& Fj = FBlocks_[pos]; // Eigen::Matrix FtE = Fj.transpose() - // * E_.block(Z * pos, 0); + // * E_.block(ZDim * pos, 0); // blocks[j] = Fj.transpose() * Fj // - FtE * PointCovariance_ * FtE.transpose(); - const Matrix23& Ej = E_.block(Z * pos, 0); + const Matrix23& Ej = E_.block(ZDim * pos, 0); blocks[j] = Fj.transpose() * (Fj - Ej * PointCovariance_ * Ej.transpose() * Fj); // F'*(I - E*P*E')*F, TODO: this should work, but it does not :-( - // static const Eigen::Matrix I2 = eye(Z); + // static const Eigen::Matrix I2 = eye(ZDim); // Matrix2 Q = // - // I2 - E_.block(Z * pos, 0) * PointCovariance_ * E_.block(Z * pos, 0).transpose(); + // I2 - E_.block(ZDim * pos, 0) * PointCovariance_ * E_.block(ZDim * pos, 0).transpose(); // blocks[j] = Fj.transpose() * Q * Fj; } return blocks; } virtual GaussianFactor::shared_ptr clone() const { - return boost::make_shared >(Fblocks_, - PointCovariance_, E_, b_); + return boost::make_shared >(keys_, + FBlocks_, PointCovariance_, E_, b_); throw std::runtime_error( "RegularImplicitSchurFactor::clone non implemented"); } @@ -353,8 +232,8 @@ public: } virtual GaussianFactor::shared_ptr negate() const { - return boost::make_shared >(Fblocks_, - PointCovariance_, E_, b_); + return boost::make_shared >(keys_, + FBlocks_, PointCovariance_, E_, b_); throw std::runtime_error( "RegularImplicitSchurFactor::negate non implemented"); } @@ -374,23 +253,24 @@ public: typedef std::vector Error2s; /** - * @brief Calculate corrected error Q*(e-Z*b) = (I - E*P*E')*(e-Z*b) + * @brief Calculate corrected error Q*(e-ZDim*b) = (I - E*P*E')*(e-ZDim*b) */ void projectError2(const Error2s& e1, Error2s& e2) const { - // d1 = E.transpose() * (e1-Z*b) = (3*2m)*2m + // d1 = E.transpose() * (e1-ZDim*b) = (3*2m)*2m Vector3 d1; d1.setZero(); for (size_t k = 0; k < size(); k++) - d1 += E_.block(Z * k, 0).transpose() - * (e1[k] - Z * b_.segment(k * Z)); + d1 += E_.block(ZDim * k, 0).transpose() + * (e1[k] - ZDim * b_.segment(k * ZDim)); // d2 = E.transpose() * e1 = (3*2m)*2m Vector3 d2 = PointCovariance_ * d1; // e3 = alpha*(e1 - E*d2) = 1*[2m-(2m*3)*3] for (size_t k = 0; k < size(); k++) - e2[k] = e1[k] - Z * b_.segment(k * Z) - E_.block(Z * k, 0) * d2; + e2[k] = e1[k] - ZDim * b_.segment(k * ZDim) + - E_.block(ZDim * k, 0) * d2; } /* @@ -410,7 +290,7 @@ public: // e1 = F * x - b = (2m*dm)*dm for (size_t k = 0; k < size(); ++k) - e1[k] = Fblocks_[k].second * x.at(keys_[k]); + e1[k] = FBlocks_[k] * x.at(keys_[k]); projectError2(e1, e2); double result = 0; @@ -432,7 +312,7 @@ public: // e1 = F * x - b = (2m*dm)*dm for (size_t k = 0; k < size(); ++k) - e1[k] = Fblocks_[k].second * x.at(keys_[k]) - b_.segment(k * Z); + e1[k] = FBlocks_[k] * x.at(keys_[k]) - b_.segment(k * ZDim); projectError(e1, e2); double result = 0; @@ -451,14 +331,14 @@ public: Vector3 d1; d1.setZero(); for (size_t k = 0; k < size(); k++) - d1 += E_.block(Z * k, 0).transpose() * e1[k]; + d1 += E_.block(ZDim * k, 0).transpose() * e1[k]; // d2 = E.transpose() * e1 = (3*2m)*2m Vector3 d2 = PointCovariance_ * d1; // e3 = alpha*(e1 - E*d2) = 1*[2m-(2m*3)*3] for (size_t k = 0; k < size(); k++) - e2[k] = e1[k] - E_.block(Z * k, 0) * d2; + e2[k] = e1[k] - E_.block(ZDim * k, 0) * d2; } /// Scratch space for multiplyHessianAdd @@ -480,19 +360,17 @@ public: e2.resize(size()); // e1 = F * x = (2m*dm)*dm - size_t k = 0; - BOOST_FOREACH(const KeyMatrix2D& it, Fblocks_) { - Key key = it.first; - e1[k++] = it.second * ConstDMap(x + D * key); + for (size_t k = 0; k < size(); ++k) { + Key key = keys_[k]; + e1[k] = FBlocks_[k] * ConstDMap(x + D * key); } projectError(e1, e2); // y += F.transpose()*e2 = (2d*2m)*2m - k = 0; - BOOST_FOREACH(const KeyMatrix2D& it, Fblocks_) { - Key key = it.first; - DMap(y + D * key) += it.second.transpose() * alpha * e2[k++]; + for (size_t k = 0; k < size(); ++k) { + Key key = keys_[k]; + DMap(y + D * key) += FBlocks_[k].transpose() * alpha * e2[k]; } } @@ -513,7 +391,7 @@ public: // e1 = F * x = (2m*dm)*dm for (size_t k = 0; k < size(); ++k) - e1[k] = Fblocks_[k].second * x.at(keys_[k]); + e1[k] = FBlocks_[k] * x.at(keys_[k]); projectError(e1, e2); @@ -525,8 +403,8 @@ public: Vector& yi = it.first->second; // Create the value as a zero vector if it does not exist. if (it.second) - yi = Vector::Zero(Fblocks_[k].second.cols()); - yi += Fblocks_[k].second.transpose() * alpha * e2[k]; + yi = Vector::Zero(FBlocks_[k].cols()); + yi += FBlocks_[k].transpose() * alpha * e2[k]; } } @@ -536,9 +414,9 @@ public: void multiplyHessianDummy(double alpha, const VectorValues& x, VectorValues& y) const { - BOOST_FOREACH(const KeyMatrix2D& Fi, Fblocks_) { + for (size_t k = 0; k < size(); ++k) { static const Vector empty; - Key key = Fi.first; + Key key = keys_[k]; std::pair it = y.tryInsert(key, empty); Vector& yi = it.first->second; yi = x.at(key); @@ -553,14 +431,14 @@ public: e1.resize(size()); e2.resize(size()); for (size_t k = 0; k < size(); k++) - e1[k] = b_.segment(Z * k); + e1[k] = b_.segment(ZDim * k); projectError(e1, e2); // g = F.transpose()*e2 VectorValues g; for (size_t k = 0; k < size(); ++k) { Key key = keys_[k]; - g.insert(key, -Fblocks_[k].second.transpose() * e2[k]); + g.insert(key, -FBlocks_[k].transpose() * e2[k]); } // return it @@ -580,12 +458,12 @@ public: e1.resize(size()); e2.resize(size()); for (size_t k = 0; k < size(); k++) - e1[k] = b_.segment(Z * k); + e1[k] = b_.segment(ZDim * k); projectError(e1, e2); for (size_t k = 0; k < size(); ++k) { // for each camera in the factor Key j = keys_[k]; - DMap(d + D * j) += -Fblocks_[k].second.transpose() * e2[k]; + DMap(d + D * j) += -FBlocks_[k].transpose() * e2[k]; } } @@ -598,9 +476,15 @@ public: }; // end class RegularImplicitSchurFactor +template +const int RegularImplicitSchurFactor::D; + +template +const int RegularImplicitSchurFactor::ZDim; + // traits -template struct traits > : public Testable< - RegularImplicitSchurFactor > { +template struct traits > : public Testable< + RegularImplicitSchurFactor > { }; } diff --git a/gtsam/slam/SmartFactorBase.h b/gtsam/slam/SmartFactorBase.h index 60e939a89..947c81385 100644 --- a/gtsam/slam/SmartFactorBase.h +++ b/gtsam/slam/SmartFactorBase.h @@ -95,8 +95,7 @@ protected: public: // Definitions for blocks of F, externally visible - typedef Eigen::Matrix Matrix2D; // F - typedef std::pair KeyMatrix2D; // Fblocks + typedef Eigen::Matrix MatrixZD; // F EIGEN_MAKE_ALIGNED_OPERATOR_NEW @@ -283,29 +282,16 @@ public: * 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. */ - double computeJacobians(std::vector& Fblocks, Matrix& E, - Vector& b, const Cameras& cameras, const Point3& point) const { - + double computeJacobians(std::vector& Fblocks, Matrix& E, Vector& b, + const Cameras& cameras, const Point3& point) const { // Project into Camera set and calculate derivatives typename Cameras::FBlocks F; b = reprojectionError(cameras, point, F, E); - - // Now calculate f and divide up the F derivatives into Fblocks - double f = 0.0; - size_t m = keys_.size(); - for (size_t i = 0, row = 0; i < m; i++, row += ZDim) { - - // Accumulate normalized error - f += b.segment(row).squaredNorm(); - - // Push piece of F onto Fblocks with associated key - Fblocks.push_back(KeyMatrix2D(keys_[i], F[i])); - } - return f; + return b.squaredNorm(); } /// SVD version - double computeJacobiansSVD(std::vector& Fblocks, Matrix& Enull, + double computeJacobiansSVD(std::vector& Fblocks, Matrix& Enull, Vector& b, const Cameras& cameras, const Point3& point) const { Matrix E; @@ -328,7 +314,7 @@ public: bool diagonalDamping = false) const { int m = this->keys_.size(); - std::vector Fblocks; + std::vector Fblocks; Matrix E; Vector b; double f = computeJacobians(Fblocks, E, b, cameras, point); @@ -342,8 +328,7 @@ public: SymmetricBlockMatrix augmentedHessian(dims, Matrix::Zero(M1, M1)); // build augmented hessian - RegularImplicitSchurFactor::SparseSchurComplement(Fblocks, E, P, b, - augmentedHessian); + CameraSet::SchurComplement(Fblocks, E, P, b, augmentedHessian); augmentedHessian(m, m)(0, 0) = f; return boost::make_shared >(keys_, @@ -361,36 +346,35 @@ public: const FastVector allKeys) const { Matrix E; Vector b; - std::vector Fblocks; - double f = computeJacobians(Fblocks, E, b, cameras, point); + std::vector Fblocks; + computeJacobians(Fblocks, E, b, cameras, point); Matrix3 P = PointCov(E, lambda, diagonalDamping); - RegularImplicitSchurFactor::UpdateSparseSchurComplement(Fblocks, - E, P, b, f, allKeys, keys_, augmentedHessian); + CameraSet::UpdateSchurComplement(Fblocks, E, P, b, allKeys, keys_, + augmentedHessian); } /// Whiten the Jacobians computed by computeJacobians using noiseModel_ - void whitenJacobians(std::vector& F, Matrix& E, - Vector& b) const { + void whitenJacobians(std::vector& F, Matrix& E, Vector& b) const { noiseModel_->WhitenSystem(E, b); // TODO make WhitenInPlace work with any dense matrix type - BOOST_FOREACH(KeyMatrix2D& Fblock,F) + BOOST_FOREACH(MatrixZD& Fblock,F) Fblock.second = noiseModel_->Whiten(Fblock.second); } /** * Return Jacobians as RegularImplicitSchurFactor with raw access */ - boost::shared_ptr > // + boost::shared_ptr > // createRegularImplicitSchurFactor(const Cameras& cameras, const Point3& point, double lambda = 0.0, bool diagonalDamping = false) const { Matrix E; Vector b; - std::vector F; + std::vector F; computeJacobians(F, E, b, cameras, point); whitenJacobians(F, E, b); Matrix3 P = PointCov(E, lambda, diagonalDamping); - return boost::make_shared >(F, E, P, - b); + return boost::make_shared >(keys_, F, E, + P, b); } /** @@ -401,7 +385,7 @@ public: bool diagonalDamping = false) const { Matrix E; Vector b; - std::vector F; + std::vector F; computeJacobians(F, E, b, cameras, point); const size_t M = b.size(); Matrix3 P = PointCov(E, lambda, diagonalDamping); @@ -416,7 +400,7 @@ public: boost::shared_ptr createJacobianSVDFactor( const Cameras& cameras, const Point3& point, double lambda = 0.0) const { size_t m = this->keys_.size(); - std::vector F; + std::vector F; Vector b; const size_t M = ZDim * m; Matrix E0(M, M - 3); @@ -427,8 +411,7 @@ public: } /// Create BIG block-diagonal matrix F from Fblocks - static void FillDiagonalF(const std::vector& Fblocks, - Matrix& F) { + static void FillDiagonalF(const std::vector& Fblocks, Matrix& F) { size_t m = Fblocks.size(); F.resize(ZDim * m, Dim * m); F.setZero(); diff --git a/gtsam/slam/tests/testRegularImplicitSchurFactor.cpp b/gtsam/slam/tests/testRegularImplicitSchurFactor.cpp index 3575a0286..4e7e0fa17 100644 --- a/gtsam/slam/tests/testRegularImplicitSchurFactor.cpp +++ b/gtsam/slam/tests/testRegularImplicitSchurFactor.cpp @@ -19,11 +19,13 @@ #include #include #include +#include +#include -#include #include #include #include +#include #include #include @@ -39,8 +41,8 @@ using namespace gtsam; const Matrix26 F0 = Matrix26::Ones(); const Matrix26 F1 = 2 * Matrix26::Ones(); const Matrix26 F3 = 3 * Matrix26::Ones(); -const vector > Fblocks = list_of > // - (make_pair(0, F0))(make_pair(1, F1))(make_pair(3, F3)); +const vector FBlocks = list_of(F0)(F1)(F3); +const FastVector keys = list_of(0)(1)(3); // RHS and sigmas const Vector b = (Vector(6) << 1., 2., 3., 4., 5., 6.).finished(); @@ -51,7 +53,7 @@ TEST( regularImplicitSchurFactor, creation ) { E.block<2,2>(0, 0) = eye(2); E.block<2,3>(2, 0) = 2 * ones(2, 3); Matrix3 P = (E.transpose() * E).inverse(); - RegularImplicitSchurFactor<6> expected(Fblocks, E, P, b); + RegularImplicitSchurFactor expected(keys, FBlocks, E, P, b); Matrix expectedP = expected.getPointCovariance(); EXPECT(assert_equal(expectedP, P)); } @@ -84,15 +86,15 @@ TEST( regularImplicitSchurFactor, addHessianMultiply ) { F << F0, zeros(2, d * 3), zeros(2, d), F1, zeros(2, d*2), zeros(2, d * 3), F3; // Calculate expected result F'*alpha*(I - E*P*E')*F*x - FastVector keys; - keys += 0,1,2,3; - Vector x = xvalues.vector(keys); + FastVector keys2; + keys2 += 0,1,2,3; + Vector x = xvalues.vector(keys2); Vector expected = zero(24); - RegularImplicitSchurFactor<6>::multiplyHessianAdd(F, E, P, alpha, x, expected); - EXPECT(assert_equal(expected, yExpected.vector(keys), 1e-8)); + RegularImplicitSchurFactor::multiplyHessianAdd(F, E, P, alpha, x, expected); + EXPECT(assert_equal(expected, yExpected.vector(keys2), 1e-8)); // Create ImplicitSchurFactor - RegularImplicitSchurFactor<6> implicitFactor(Fblocks, E, P, b); + RegularImplicitSchurFactor implicitFactor(keys, FBlocks, E, P, b); VectorValues zero = 0 * yExpected;// quick way to get zero w right structure { // First Version @@ -122,7 +124,7 @@ TEST( regularImplicitSchurFactor, addHessianMultiply ) { // Create JacobianFactor with same error const SharedDiagonal model; - JacobianFactorQ<6, 2> jf(Fblocks, E, P, b, model); + JacobianFactorQ<6, 2> jf(keys, FBlocks, E, P, b, model); { // error double expectedError = jf.error(xvalues); @@ -172,7 +174,7 @@ TEST( regularImplicitSchurFactor, addHessianMultiply ) { } // Create JacobianFactorQR - JacobianFactorQR<6, 2> jfq(Fblocks, E, P, b, model); + JacobianFactorQR<6, 2> jfq(keys, FBlocks, E, P, b, model); { const SharedDiagonal model; VectorValues yActual = zero; @@ -214,7 +216,7 @@ TEST(regularImplicitSchurFactor, hessianDiagonal) E.block<2,3>(2, 0) << 1,2,3,4,5,6; E.block<2,3>(4, 0) << 0.5,1,2,3,4,5; Matrix3 P = (E.transpose() * E).inverse(); - RegularImplicitSchurFactor<6> factor(Fblocks, E, P, b); + RegularImplicitSchurFactor factor(keys, FBlocks, E, P, b); // hessianDiagonal VectorValues expected;