From 91a6613d84e786d2cf76f63d1c52bb0025730f31 Mon Sep 17 00:00:00 2001 From: lcarlone Date: Thu, 22 Jul 2021 22:31:33 -0400 Subject: [PATCH] moved common function to cameraSet. commented issues with templated calls to functions in cameraSet --- gtsam/geometry/CameraSet.h | 99 +++++++++++++++ .../SmartProjectionPoseFactorRollingShutter.h | 114 +++++------------- .../slam/SmartStereoProjectionFactorPP.h | 92 +++----------- 3 files changed, 141 insertions(+), 164 deletions(-) diff --git a/gtsam/geometry/CameraSet.h b/gtsam/geometry/CameraSet.h index 2b656bd35..19ab7ab31 100644 --- a/gtsam/geometry/CameraSet.h +++ b/gtsam/geometry/CameraSet.h @@ -200,6 +200,105 @@ public: 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) + * In this version, we allow for the case where the keys in the Jacobian are organized + * differents from the keys in the output SymmetricBlockMatrix + * In particular: each block of the Jacobian captures 2 poses (useful for rolling shutter and extrinsic calibration) + */ + template // N = 2 or 3 (point dimension), ND is the Jacobian block dimension, NDD is the Hessian block dimension + static SymmetricBlockMatrix SchurComplementAndRearrangeBlocks( + const std::vector, + Eigen::aligned_allocator > >& Fs, + const Matrix& E, const Eigen::Matrix& P, const Vector& b, + const KeyVector jacobianKeys, const KeyVector hessianKeys) { + + size_t nrNonuniqueKeys = jacobianKeys.size(); + size_t nrUniqueKeys = hessianKeys.size(); + + // marginalize point: note - we reuse the standard SchurComplement function + SymmetricBlockMatrix augmentedHessian = SchurComplement(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, NDD); + dims.back() = 1; + SymmetricBlockMatrix augmentedHessianUniqueKeys; + + // here we have to deal with the fact that some blocks may share the same keys + 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, NDD); + nonuniqueDims.back() = 1; + augmentedHessian = SymmetricBlockMatrix( + nonuniqueDims, Matrix(augmentedHessian.selfadjointView())); + + // 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[hessianKeys[k]] = k; + } + + // initialize matrix to zero + augmentedHessianUniqueKeys = SymmetricBlockMatrix( + dims, Matrix::Zero(NDD * nrUniqueKeys + 1, NDD * 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 = jacobianKeys.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 = jacobianKeys.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 augmentedHessianUniqueKeys; + } + + /** + * non-templated version of function above + */ + static SymmetricBlockMatrix SchurComplementAndRearrangeBlocks_3_12_6( + const std::vector, + Eigen::aligned_allocator > >& Fs, + const Matrix& E, const Eigen::Matrix& P, const Vector& b, + const KeyVector jacobianKeys, const KeyVector hessianKeys) { + return SchurComplementAndRearrangeBlocks<3,12,6>(Fs, E, P, b, + jacobianKeys, + hessianKeys); + } + /** * Do Schur complement, given Jacobian as Fs,E,P, return SymmetricBlockMatrix * G = F' * F - F' * E * P * E' * F diff --git a/gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.h b/gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.h index 3393f7ca4..c35beb0e2 100644 --- a/gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.h +++ b/gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.h @@ -62,8 +62,6 @@ PinholePose > { /// shorthand for base class type typedef SmartProjectionFactor > Base; -// typedef PinholePose Camera; -// typedef CameraSet Cameras; /// shorthand for this class typedef SmartProjectionPoseFactorRollingShutter This; @@ -299,27 +297,27 @@ PinholePose > { // we may have multiple observation sharing the same keys (due to the rolling shutter interpolation), // hence the number of unique keys may be smaller than 2 * nrMeasurements - size_t nrUniqueKeys = this->keys_.size(); - size_t nrNonuniqueKeys = 2*world_P_body_key_pairs_.size(); + size_t nrUniqueKeys = this->keys_.size(); // Create structures for Hessian Factors KeyVector js; std::vector < Matrix > Gs(nrUniqueKeys * (nrUniqueKeys + 1) / 2); - std::vector gs(nrUniqueKeys); + std::vector < Vector > gs(nrUniqueKeys); if (this->measured_.size() != this->cameras(values).size()) throw std::runtime_error("SmartProjectionPoseFactorRollingShutter: " - "measured_.size() inconsistent with input"); + "measured_.size() inconsistent with input"); // triangulate 3D point at given linearization point this->triangulateSafe(this->cameras(values)); - if (!this->result_) { // failed: return "empty/zero" Hessian + if (!this->result_) { // failed: return "empty/zero" Hessian 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); + return boost::make_shared < RegularHessianFactor + > (this->keys_, Gs, gs, 0.0); } // compute Jacobian given triangulated 3D Point @@ -333,88 +331,32 @@ 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); + Matrix3 P = Base::Cameras::PointCov(E, lambda, diagonalDamping); + // the following unfortunately does not seem to work and causes + // an "reference to overloaded function could not be resolved; did you mean to call it?" error + // Matrix3 P; + // Base::Cameras::ComputePointCovariance<3>(P, E, lambda, diagonalDamping); // build augmented Hessian (with last row/column being the information vector) - // 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); - - SymmetricBlockMatrix augmentedHessian = - Base::Cameras::SchurComplement312(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); // 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)); + // 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); } + // but we need to get the augumented hessian wrt the unique keys in key_ + SymmetricBlockMatrix augmentedHessianUniqueKeys = + Base::Cameras::SchurComplementAndRearrangeBlocks_3_12_6( + Fs, E, P, b, nonuniqueKeys, this->keys_); + + // the following unfortunately does not seem to work and causes + // an "reference to overloaded function could not be resolved; did you mean to call it?" error + // SymmetricBlockMatrix augmentedHessianUniqueKeys = + // Base::Cameras::SchurComplementAndRearrangeBlocks<3, DimBlock, DimPose>( + // Fs, E, P, b, nonuniqueKeys, keys_); + return boost::make_shared < RegularHessianFactor - > ( this->keys_, augmentedHessianUniqueKeys); + > (this->keys_, augmentedHessianUniqueKeys); } /** diff --git a/gtsam_unstable/slam/SmartStereoProjectionFactorPP.h b/gtsam_unstable/slam/SmartStereoProjectionFactorPP.h index 40d90d614..25be48b0f 100644 --- a/gtsam_unstable/slam/SmartStereoProjectionFactorPP.h +++ b/gtsam_unstable/slam/SmartStereoProjectionFactorPP.h @@ -61,10 +61,10 @@ class SmartStereoProjectionFactorPP : public SmartStereoProjectionFactor { /// shorthand for a smart pointer to a factor typedef boost::shared_ptr shared_ptr; - static const int Dim = 12; ///< Camera dimension: 6 for body pose, 6 for extrinsic pose + static const int DimBlock = 12; ///< Camera dimension: 6 for body pose, 6 for extrinsic pose static const int DimPose = 6; ///< Pose3 dimension static const int ZDim = 3; ///< Measurement dimension (for a StereoPoint2 measurement) - typedef Eigen::Matrix MatrixZD; // F blocks (derivatives wrt camera) + typedef Eigen::Matrix MatrixZD; // F blocks (derivatives wrt camera) typedef std::vector > FBlocks; // vector of F blocks /** @@ -180,7 +180,7 @@ class SmartStereoProjectionFactorPP : public SmartStereoProjectionFactor { // get jacobians and error vector for current measurement StereoPoint2 reprojectionError_i = StereoPoint2( camera.project(*result_, dProject_dPoseCam_i, Ei) - measured_.at(i)); - Eigen::Matrix J; // 3 x 12 + Eigen::Matrix J; // 3 x 12 J.block(0, 0) = dProject_dPoseCam_i * dPoseCam_dPoseBody_i; // (3x6) * (6x6) J.block(0, 6) = dProject_dPoseCam_i * dPoseCam_dPoseExt_i; // (3x6) * (6x6) // if the right pixel is invalid, fix jacobians @@ -209,8 +209,6 @@ class SmartStereoProjectionFactorPP : public SmartStereoProjectionFactor { // of keys may be smaller than 2 * nrMeasurements (which is the upper bound where we // have a body key and an extrinsic calibration key for each measurement) size_t nrUniqueKeys = keys_.size(); - size_t nrNonuniqueKeys = world_P_body_keys_.size() - + body_P_cam_keys_.size(); // Create structures for Hessian Factors KeyVector js; @@ -246,81 +244,19 @@ class SmartStereoProjectionFactorPP : public SmartStereoProjectionFactor { // build augmented Hessian (with last row/column being the information vector) Matrix3 P; - Cameras::ComputePointCovariance<3>(P, E, lambda, diagonalDamping); + Cameras::ComputePointCovariance <3> (P, E, lambda, diagonalDamping); - // marginalize point: note - we reuse the standard SchurComplement function - SymmetricBlockMatrix augmentedHessian = - Cameras::SchurComplement<3, Dim>(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_keys_.size(); i++) { - nonuniqueKeys.push_back(world_P_body_keys_.at(i)); - nonuniqueKeys.push_back(body_P_cam_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[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)); + // 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_keys_.size(); i++) { + nonuniqueKeys.push_back(world_P_body_keys_.at(i)); + nonuniqueKeys.push_back(body_P_cam_keys_.at(i)); } + // but we need to get the augumented hessian wrt the unique keys in key_ + SymmetricBlockMatrix augmentedHessianUniqueKeys = + Cameras::SchurComplementAndRearrangeBlocks<3,DimBlock,DimPose>(Fs,E,P,b, + nonuniqueKeys, keys_); + return boost::make_shared < RegularHessianFactor > (keys_, augmentedHessianUniqueKeys); }