diff --git a/gtsam/geometry/CameraSet.h b/gtsam/geometry/CameraSet.h index 7d2f818fa..fcbff020c 100644 --- a/gtsam/geometry/CameraSet.h +++ b/gtsam/geometry/CameraSet.h @@ -142,14 +142,67 @@ public: return ErrorVector(project2(point, Fs, E), measured); } + static SymmetricBlockMatrix mySchurComplement( + const std::vector< Eigen::Matrix, + Eigen::aligned_allocator< Eigen::Matrix > >& Fs, + const Matrix& E, const Eigen::Matrix& P, const Vector& b) { + return mySchurComplement<2,3,12>(Fs, E, P, b); + } + + template // N = 2 or 3 (point dimension), ND is the camera dimension + static SymmetricBlockMatrix mySchurComplement( + const std::vector< Eigen::Matrix, + Eigen::aligned_allocator< Eigen::Matrix > >& Fs, + const Matrix& E, const Eigen::Matrix& P, const Vector& b) { + + // a single point is observed in m cameras + size_t m = Fs.size(); + + // Create a SymmetricBlockMatrix (augmented hessian, with extra row/column with info vector) + size_t M1 = ND * m + 1; + std::vector dims(m + 1); // this also includes the b term + std::fill(dims.begin(), dims.end() - 1, ND); + dims.back() = 1; + SymmetricBlockMatrix augmentedHessian(dims, Matrix::Zero(M1, M1)); + + // Blockwise Schur complement + for (size_t i = 0; i < m; i++) { // for each camera + + const Eigen::Matrix& Fi = Fs[i]; + const auto FiT = Fi.transpose(); + const Eigen::Matrix Ei_P = // + E.block(myZDim * i, 0, myZDim, N) * P; + + // D = (Dx2) * ZDim + augmentedHessian.setOffDiagonalBlock(i, m, FiT * b.segment(myZDim * i) // F' * b + - FiT * (Ei_P * (E.transpose() * b))); // D = (DxZDim) * (ZDimx3) * (N*ZDimm) * (ZDimm x 1) + + // (DxD) = (DxZDim) * ( (ZDimxD) - (ZDimx3) * (3xZDim) * (ZDimxD) ) + augmentedHessian.setDiagonalBlock(i, FiT + * (Fi - Ei_P * E.block(myZDim * i, 0, myZDim, N).transpose() * Fi)); + + // upper triangular part of the hessian + for (size_t j = i + 1; j < m; j++) { // for each camera + const Eigen::Matrix& Fj = Fs[j]; + + // (DxD) = (Dx2) * ( (2x2) * (2xD) ) + augmentedHessian.setOffDiagonalBlock(i, j, -FiT + * (Ei_P * E.block(myZDim * j, 0, myZDim, N).transpose() * Fj)); + } + } // end of for over cameras + + augmentedHessian.diagonalBlock(m)(0, 0) += b.squaredNorm(); + return augmentedHessian; + } + /** * Do Schur complement, given Jacobian as Fs,E,P, return SymmetricBlockMatrix * G = F' * F - F' * E * P * E' * F * g = F' * (b - E * P * E' * b) * Fixed size version */ - template // N = 2 or 3, ND is the camera dimension - static SymmetricBlockMatrix SchurComplement( + template // N = 2 or 3 (point dimension), ND is the camera dimension + static SymmetricBlockMatrix SchurComplementWithCustomBlocks( const std::vector< Eigen::Matrix, Eigen::aligned_allocator< Eigen::Matrix > >& Fs, const Matrix& E, const Eigen::Matrix& P, const Vector& b) { @@ -202,11 +255,11 @@ public: template // N = 2 or 3 static SymmetricBlockMatrix SchurComplement(const FBlocks& Fs, const Matrix& E, const Eigen::Matrix& P, const Vector& b) { - return SchurComplement(Fs, E, P, b); + return SchurComplementWithCustomBlocks(Fs, E, P, b); } /// Computes Point Covariance P, with lambda parameter - template // N = 2 or 3 + template // N = 2 or 3 (point dimension) static void ComputePointCovariance(Eigen::Matrix& P, const Matrix& E, double lambda, bool diagonalDamping = false) { @@ -258,7 +311,7 @@ public: * 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. */ - template // N = 2 or 3 + template // N = 2 or 3 (point dimension) static void UpdateSchurComplement(const FBlocks& Fs, const Matrix& E, const Eigen::Matrix& P, const Vector& b, const KeyVector& allKeys, const KeyVector& keys, diff --git a/gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.h b/gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.h index a9f6d6bdf..472b6348a 100644 --- a/gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.h +++ b/gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.h @@ -17,10 +17,10 @@ #pragma once -#include -#include -#include #include +#include +#include +#include namespace gtsam { /** @@ -60,11 +60,10 @@ PinholePose > { public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW - typedef PinholePose Camera; - // typedef CameraSet Cameras; - /// shorthand for base class type - typedef SmartProjectionFactor Base; + typedef SmartProjectionFactor > Base; +// typedef PinholePose Camera; +// typedef CameraSet Cameras; /// shorthand for this class typedef SmartProjectionPoseFactorRollingShutter This; @@ -312,8 +311,6 @@ PinholePose > { throw std::runtime_error("SmartProjectionPoseFactorRollingShutter: " "measured_.size() inconsistent with input"); - std::cout << "got this far.." << std::endl; - // triangulate 3D point at given linearization point this->triangulateSafe(this->cameras(values)); @@ -336,91 +333,95 @@ PinholePose > { for (size_t i = 0; i < Fs.size(); i++) Fs[i] = this->noiseModel_->Whiten(Fs[i]); + Matrix3 P = Base::Cameras::PointCov(E, lambda, diagonalDamping); + // build augmented Hessian (with last row/column being the information vector) - Matrix3 P; - Cameras::ComputePointCovariance<3>(P, E, lambda, diagonalDamping); + // and marginalize point: note - we reuse the standard SchurComplement function + // does not work, since assumes convensional 6-dim blocks + // SymmetricBlockMatrix augmentedHessian = + // Base::Cameras::SchurComplement(Fs, E, b, lambda,diagonalDamping); - // // marginalize point: note - we reuse the standard SchurComplement function - // SymmetricBlockMatrix augmentedHessian = This::Cameras::SchurComplement<2,DimBlock>(Fs, E, P, b); + SymmetricBlockMatrix augmentedHessian = + Base::Cameras::mySchurComplement(Fs, E, P, b); - // // now pack into an Hessian factor - // std::vector dims(nrUniqueKeys + 1); // this also includes the b term - // std::fill(dims.begin(), dims.end() - 1, 6); - // dims.back() = 1; - // SymmetricBlockMatrix augmentedHessianUniqueKeys; - // - // // here we have to deal with the fact that some cameras may share the same extrinsic key - // if (nrUniqueKeys == nrNonuniqueKeys) { // if there is 1 calibration key per camera - // augmentedHessianUniqueKeys = SymmetricBlockMatrix( - // dims, Matrix(augmentedHessian.selfadjointView())); - // } else { // if multiple cameras share a calibration we have to rearrange - // // the results of the Schur complement matrix - // std::vector nonuniqueDims(nrNonuniqueKeys + 1); // this also includes the b term - // std::fill(nonuniqueDims.begin(), nonuniqueDims.end() - 1, 6); - // nonuniqueDims.back() = 1; - // augmentedHessian = SymmetricBlockMatrix( - // nonuniqueDims, Matrix(augmentedHessian.selfadjointView())); - // - // // these are the keys that correspond to the blocks in augmentedHessian (output of SchurComplement) - // KeyVector nonuniqueKeys; - // for (size_t i = 0; i < world_P_body_key_pairs_.size(); i++) { - // nonuniqueKeys.push_back(world_P_body_key_pairs_.at(i)); - // nonuniqueKeys.push_back(body_P_cam_ this->keys_.at(i)); - // } - // - // // get map from key to location in the new augmented Hessian matrix (the one including only unique keys) - // std::map keyToSlotMap; - // for (size_t k = 0; k < nrUniqueKeys; k++) { - // keyToSlotMap[ this->keys_[k]] = k; - // } - // - // // initialize matrix to zero - // augmentedHessianUniqueKeys = SymmetricBlockMatrix( - // dims, Matrix::Zero(6 * nrUniqueKeys + 1, 6 * nrUniqueKeys + 1)); - // - // // add contributions for each key: note this loops over the hessian with nonUnique keys (augmentedHessian) - // // and populates an Hessian that only includes the unique keys (that is what we want to return) - // for (size_t i = 0; i < nrNonuniqueKeys; i++) { // rows - // Key key_i = nonuniqueKeys.at(i); - // - // // update information vector - // augmentedHessianUniqueKeys.updateOffDiagonalBlock( - // keyToSlotMap[key_i], nrUniqueKeys, - // augmentedHessian.aboveDiagonalBlock(i, nrNonuniqueKeys)); - // - // // update blocks - // for (size_t j = i; j < nrNonuniqueKeys; j++) { // cols - // Key key_j = nonuniqueKeys.at(j); - // if (i == j) { - // augmentedHessianUniqueKeys.updateDiagonalBlock( - // keyToSlotMap[key_i], augmentedHessian.diagonalBlock(i)); - // } else { // (i < j) - // if (keyToSlotMap[key_i] != keyToSlotMap[key_j]) { - // augmentedHessianUniqueKeys.updateOffDiagonalBlock( - // keyToSlotMap[key_i], keyToSlotMap[key_j], - // augmentedHessian.aboveDiagonalBlock(i, j)); - // } else { - // augmentedHessianUniqueKeys.updateDiagonalBlock( - // keyToSlotMap[key_i], - // augmentedHessian.aboveDiagonalBlock(i, j) - // + augmentedHessian.aboveDiagonalBlock(i, j).transpose()); - // } - // } - // } - // } - // // update bottom right element of the matrix - // augmentedHessianUniqueKeys.updateDiagonalBlock( - // nrUniqueKeys, augmentedHessian.diagonalBlock(nrNonuniqueKeys)); - // } - // return boost::make_shared < RegularHessianFactor - // > ( this->keys_, augmentedHessianUniqueKeys); + // now pack into an Hessian factor + std::vector dims(nrUniqueKeys + 1); // this also includes the b term + std::fill(dims.begin(), dims.end() - 1, 6); + dims.back() = 1; + SymmetricBlockMatrix augmentedHessianUniqueKeys; - // TO REMOVE: - for (Matrix& m : Gs) - m = Matrix::Zero(DimPose, DimPose); - for (Vector& v : gs) - v = Vector::Zero(DimPose); - return boost::make_shared < RegularHessianFactor > ( this->keys_, Gs, gs, 0.0); + // here we have to deal with the fact that some cameras may share the same extrinsic key + if (nrUniqueKeys == nrNonuniqueKeys) { // if there is 1 calibration key per camera + augmentedHessianUniqueKeys = SymmetricBlockMatrix( + dims, Matrix(augmentedHessian.selfadjointView())); + } else { // if multiple cameras share a calibration we have to rearrange + // the results of the Schur complement matrix + std::vector nonuniqueDims(nrNonuniqueKeys + 1); // this also includes the b term + std::fill(nonuniqueDims.begin(), nonuniqueDims.end() - 1, 6); // all are poses .. + nonuniqueDims.back() = 1; // except b is a scalar + augmentedHessian = SymmetricBlockMatrix( + nonuniqueDims, Matrix(augmentedHessian.selfadjointView())); + + // these are the keys that correspond to the blocks in augmentedHessian (output of SchurComplement) + KeyVector nonuniqueKeys; + for (size_t i = 0; i < world_P_body_key_pairs_.size(); i++) { + nonuniqueKeys.push_back(world_P_body_key_pairs_.at(i).first); + nonuniqueKeys.push_back(world_P_body_key_pairs_.at(i).second); + } + + // get map from key to location in the new augmented Hessian matrix (the one including only unique keys) + std::map keyToSlotMap; + for (size_t k = 0; k < nrUniqueKeys; k++) { + keyToSlotMap[ this->keys_[k] ] = k; + } + + // initialize matrix to zero + augmentedHessianUniqueKeys = SymmetricBlockMatrix( + dims, Matrix::Zero(6 * nrUniqueKeys + 1, 6 * nrUniqueKeys + 1)); + + // add contributions for each key: note this loops over the hessian with nonUnique keys (augmentedHessian) + // and populates an Hessian that only includes the unique keys (that is what we want to return) + for (size_t i = 0; i < nrNonuniqueKeys; i++) { // rows + Key key_i = nonuniqueKeys.at(i); + + // update information vector + augmentedHessianUniqueKeys.updateOffDiagonalBlock( + keyToSlotMap[key_i], nrUniqueKeys, + augmentedHessian.aboveDiagonalBlock(i, nrNonuniqueKeys)); + + // update blocks + for (size_t j = i; j < nrNonuniqueKeys; j++) { // cols + Key key_j = nonuniqueKeys.at(j); + if (i == j) { + augmentedHessianUniqueKeys.updateDiagonalBlock( + keyToSlotMap[key_i], augmentedHessian.diagonalBlock(i)); + } else { // (i < j) + if (keyToSlotMap[key_i] != keyToSlotMap[key_j]) { + augmentedHessianUniqueKeys.updateOffDiagonalBlock( + keyToSlotMap[key_i], keyToSlotMap[key_j], + augmentedHessian.aboveDiagonalBlock(i, j)); + } else { + augmentedHessianUniqueKeys.updateDiagonalBlock( + keyToSlotMap[key_i], + augmentedHessian.aboveDiagonalBlock(i, j) + + augmentedHessian.aboveDiagonalBlock(i, j).transpose()); + } + } + } + } + // update bottom right element of the matrix + augmentedHessianUniqueKeys.updateDiagonalBlock( + nrUniqueKeys, augmentedHessian.diagonalBlock(nrNonuniqueKeys)); + } + return boost::make_shared < RegularHessianFactor + > ( this->keys_, augmentedHessianUniqueKeys); + +// // TO REMOVE: +// for (Matrix& m : Gs) +// m = Matrix::Zero(DimPose, DimPose); +// for (Vector& v : gs) +// v = Vector::Zero(DimPose); +// return boost::make_shared < RegularHessianFactor > ( this->keys_, Gs, gs, 0.0); } /** @@ -470,7 +471,7 @@ PinholePose > { // depending on flag set on construction we may linearize to different linear factors switch (this->params_.linearizationMode) { case HESSIAN: - return createHessianFactor(values, lambda); + return this->createHessianFactor(values, lambda); default: throw std::runtime_error( "SmartProjectionPoseFactorRollingShutter: unknown linearization mode"); @@ -480,7 +481,7 @@ PinholePose > { /// linearize boost::shared_ptr linearize(const Values& values) const override { - return linearizeDamped(values); + return this->linearizeDamped(values); } private: diff --git a/gtsam_unstable/slam/SmartStereoProjectionFactorPP.h b/gtsam_unstable/slam/SmartStereoProjectionFactorPP.h index 40d90d614..e4d714b0f 100644 --- a/gtsam_unstable/slam/SmartStereoProjectionFactorPP.h +++ b/gtsam_unstable/slam/SmartStereoProjectionFactorPP.h @@ -250,7 +250,7 @@ class SmartStereoProjectionFactorPP : public SmartStereoProjectionFactor { // marginalize point: note - we reuse the standard SchurComplement function SymmetricBlockMatrix augmentedHessian = - Cameras::SchurComplement<3, Dim>(Fs, E, P, b); + Cameras::SchurComplementWithCustomBlocks<3, Dim>(Fs, E, P, b); // now pack into an Hessian factor std::vector dims(nrUniqueKeys + 1); // this also includes the b term