12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

all pass!

release/4.3a0
lcarlone 2021-07-22 21:49:40 -04:00
parent d7e8912d6a
commit 477dd5b247
3 changed files with 8 additions and 61 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

58
gtsam/geometry/CameraSet.h
View File

@ -142,57 +142,11 @@ public:
return ErrorVector(project2(point, Fs, E), measured);
}
static SymmetricBlockMatrix mySchurComplement(
const std::vector< Eigen::Matrix<double, 2, 12>,
Eigen::aligned_allocator< Eigen::Matrix<double, 2, 12> > >& Fs,
static SymmetricBlockMatrix SchurComplement312(
const std::vector< Eigen::Matrix<double, ZDim, 12>,
Eigen::aligned_allocator< Eigen::Matrix<double, ZDim, 12> > >& Fs,
const Matrix& E, const Eigen::Matrix<double, 3, 3>& P, const Vector& b) {
return mySchurComplement<2,3,12>(Fs, E, P, b);
}
template<int myZDim, int N, int ND> // N = 2 or 3 (point dimension), ND is the camera dimension
static SymmetricBlockMatrix mySchurComplement(
const std::vector< Eigen::Matrix<double, myZDim, ND>,
Eigen::aligned_allocator< Eigen::Matrix<double, myZDim, ND> > >& Fs,
const Matrix& E, const Eigen::Matrix<double, N, N>& 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<DenseIndex> 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<double, myZDim, ND>& Fi = Fs[i];
const auto FiT = Fi.transpose();
const Eigen::Matrix<double, myZDim, N> Ei_P = //
E.block(myZDim * i, 0, myZDim, N) * P;
// D = (Dx2) * ZDim
augmentedHessian.setOffDiagonalBlock(i, m, FiT * b.segment<myZDim>(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<double, myZDim, ND>& 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;
return SchurComplement<3,12>(Fs, E, P, b);
}
/**
@ -202,7 +156,7 @@ public:
* Fixed size version
*/
template<int N, int ND> // N = 2 or 3 (point dimension), ND is the camera dimension
static SymmetricBlockMatrix SchurComplementWithCustomBlocks(
static SymmetricBlockMatrix SchurComplement(
const std::vector< Eigen::Matrix<double, ZDim, ND>, Eigen::aligned_allocator< Eigen::Matrix<double, ZDim, ND> > >& Fs,
const Matrix& E, const Eigen::Matrix<double, N, N>& P, const Vector& b) {
@ -255,7 +209,7 @@ public:
template<int N> // N = 2 or 3
static SymmetricBlockMatrix SchurComplement(const FBlocks& Fs,
const Matrix& E, const Eigen::Matrix<double, N, N>& P, const Vector& b) {
return SchurComplementWithCustomBlocks<N,D>(Fs, E, P, b);
return SchurComplement<N,D>(Fs, E, P, b);
}
/// Computes Point Covariance P, with lambda parameter

9
gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.h
View File

@ -342,7 +342,7 @@ PinholePose<CALIBRATION> > {
// Base::Cameras::SchurComplement(Fs, E, b, lambda,diagonalDamping);
SymmetricBlockMatrix augmentedHessian =
Base::Cameras::mySchurComplement(Fs, E, P, b);
Base::Cameras::SchurComplement312(Fs, E, P, b);
// now pack into an Hessian factor
std::vector<DenseIndex> dims(nrUniqueKeys + 1); // this also includes the b term
@ -415,13 +415,6 @@ PinholePose<CALIBRATION> > {
}
return boost::make_shared < RegularHessianFactor<DimPose>
> ( 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<DimPose> > ( this->keys_, Gs, gs, 0.0);
}
/**

2
gtsam_unstable/slam/SmartStereoProjectionFactorPP.h
View File

@ -250,7 +250,7 @@ class SmartStereoProjectionFactorPP : public SmartStereoProjectionFactor {
// marginalize point: note - we reuse the standard SchurComplement function
SymmetricBlockMatrix augmentedHessian =
Cameras::SchurComplementWithCustomBlocks<3, Dim>(Fs, E, P, b);
Cameras::SchurComplement<3, Dim>(Fs, E, P, b);
// now pack into an Hessian factor
std::vector<DenseIndex> dims(nrUniqueKeys + 1); // this also includes the b term