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

isolated schur complement!

release/4.3a0
lcarlone 2021-03-13 21:51:39 -05:00
parent 0161047427
commit dbc10ff227
3 changed files with 66 additions and 48 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

90
gtsam/geometry/CameraSet.h
View File

@ -139,6 +139,56 @@ public:
return ErrorVector(project2(point, Fs, E), measured);
}
/**
* 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<int N, int ND> // N = 2 or 3, ND is the camera dimension
static SymmetricBlockMatrix SchurComplement(const FBlocks& 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
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 MatrixZD& Fi = Fs[i];
const auto FiT = Fi.transpose();
const Eigen::Matrix<double, ZDim, N> Ei_P = //
E.block(ZDim * i, 0, ZDim, N) * P;
// D = (Dx2) * ZDim
augmentedHessian.setOffDiagonalBlock(i, m, FiT * b.segment<ZDim>(ZDim * 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(ZDim * i, 0, ZDim, N).transpose() * Fi));
// upper triangular part of the hessian
for (size_t j = i + 1; j < m; j++) { // for each camera
const MatrixZD& Fj = Fs[j];
// (DxD) = (Dx2) * ( (2x2) * (2xD) )
augmentedHessian.setOffDiagonalBlock(i, j, -FiT
* (Ei_P * E.block(ZDim * j, 0, ZDim, 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
@ -148,45 +198,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) {
// a single point is observed in m cameras
size_t m = Fs.size();
// Create a SymmetricBlockMatrix
size_t M1 = D * m + 1;
std::vector<DenseIndex> 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));
// Blockwise Schur complement
for (size_t i = 0; i < m; i++) { // for each camera
const MatrixZD& Fi = Fs[i];
const auto FiT = Fi.transpose();
const Eigen::Matrix<double, ZDim, N> Ei_P = //
E.block(ZDim * i, 0, ZDim, N) * P;
// D = (Dx2) * ZDim
augmentedHessian.setOffDiagonalBlock(i, m, FiT * b.segment<ZDim>(ZDim * 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(ZDim * i, 0, ZDim, N).transpose() * Fi));
// upper triangular part of the hessian
for (size_t j = i + 1; j < m; j++) { // for each camera
const MatrixZD& Fj = Fs[j];
// (DxD) = (Dx2) * ( (2x2) * (2xD) )
augmentedHessian.setOffDiagonalBlock(i, j, -FiT
* (Ei_P * E.block(ZDim * j, 0, ZDim, N).transpose() * Fj));
}
} // end of for over cameras
augmentedHessian.diagonalBlock(m)(0, 0) += b.squaredNorm();
return augmentedHessian;
return SchurComplement<N,D>(Fs, E, P, b);
}
/// Computes Point Covariance P, with lambda parameter

6
gtsam_unstable/slam/SmartStereoProjectionFactorPP.h
View File

@ -154,6 +154,12 @@ class SmartStereoProjectionFactorPP : public SmartStereoProjectionFactor {
if (!result_) {
throw ("computeJacobiansWithTriangulatedPoint");
} else {
// Matrix H0, H02;
// PinholeCamera<CALIBRATION> camera(pose.compose(transform, H0, H02), *K_);
// Point2 reprojectionError(camera.project(point, H1, H3, boost::none) - measured_);
// *H2 = *H1 * H02;
// *H1 = *H1 * H0;
// valid result: compute jacobians
// const Pose3 sensor_P_body = body_P_sensor_->inverse();
// constexpr int camera_dim = traits<CAMERA>::dimension;

18
gtsam_unstable/slam/tests/testSmartStereoProjectionFactorPP.cpp
View File

@ -462,15 +462,15 @@ TEST( SmartStereoProjectionFactorPP, 3poses_smart_projection_factor ) {
Point3 landmark2_smart = *smartFactor2->point();
Point3 landmark3_smart = *smartFactor3->point();
Values result;
gttic_(SmartStereoProjectionFactorPP);
LevenbergMarquardtOptimizer optimizer(graph, values, lm_params);
result = optimizer.optimize();
gttoc_(SmartStereoProjectionFactorPP);
tictoc_finishedIteration_();
// cout << std::setprecision(10) << "SmartStereoFactor graph optimized error: " << graph.error(result) << endl;
EXPECT_DOUBLES_EQUAL(0, graph.error(result), 1e-5);
// Values result;
// gttic_(SmartStereoProjectionFactorPP);
// LevenbergMarquardtOptimizer optimizer(graph, values, lm_params);
// result = optimizer.optimize();
// gttoc_(SmartStereoProjectionFactorPP);
// tictoc_finishedIteration_();
//
// // cout << std::setprecision(10) << "SmartStereoFactor graph optimized error: " << graph.error(result) << endl;
// EXPECT_DOUBLES_EQUAL(0, graph.error(result), 1e-5);
// GaussianFactorGraph::shared_ptr GFG = graph.linearize(result);
// VectorValues delta = GFG->optimize();