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

test still in progress; removed a tmp function

release/4.3a0
lcarlone 2021-07-24 00:15:33 -04:00
parent e4f1bb1bd0
commit 9834042040
2 changed files with 87 additions and 7 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

7
gtsam/geometry/CameraSet.h
View File

@ -142,13 +142,6 @@ public:
return ErrorVector(project2(point, Fs, E), measured);
}
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 SchurComplement<3,12>(Fs, E, P, b);
}
/**
* Do Schur complement, given Jacobian as Fs,E,P, return SymmetricBlockMatrix
* G = F' * F - F' * E * P * E' * F

87
gtsam/geometry/tests/testCameraSet.cpp
View File

@ -125,6 +125,93 @@ TEST(CameraSet, Pinhole) {
EXPECT(assert_equal(actualE, E));
}
/* ************************************************************************* */
TEST(CameraSet, SchurComplementAndRearrangeBlocks) {
typedef PinholePose<Cal3Bundler> Camera;
typedef CameraSet<Camera> Set;
typedef Point2Vector ZZ;
KeyVector nonuniqueKeys;
nonuniqueKeys.push_back(0);
nonuniqueKeys.push_back(1);
nonuniqueKeys.push_back(1);
nonuniqueKeys.push_back(2);
nonuniqueKeys.push_back(2);
nonuniqueKeys.push_back(0);
KeyVector uniqueKeys;
uniqueKeys.push_back(0);
uniqueKeys.push_back(1);
uniqueKeys.push_back(2);
// this is the (block) Jacobian with respect to the nonuniqueKeys
std::vector<Eigen::Matrix<double,2, 12>,
Eigen::aligned_allocator<Eigen::Matrix<double,2,12> > > Fs;
Fs.push_back(1 * Matrix::Ones(2,12)); // corresponding to key pair (0,1)
Fs.push_back(2 * Matrix::Ones(2,12)); // corresponding to key pair (1,2)
Fs.push_back(3 * Matrix::Ones(2,12)); // corresponding to key pair (2,0)
Matrix E = Matrix::Identity(6,3) + Matrix::Ones(6,3);
Matrix34 Et = E.transpose();
Matrix P = (Et * E).inverse();
Vector b = 5*Vector::Ones(6);
// { // SchurComplement
// // Actual
// SymmetricBlockMatrix augmentedHessianBM = Set::SchurComplement<3,12>(Fs,E,P,b);
// Matrix actualAugmentedHessian = augmentedHessianBM.selfadjointView();
//
// // Expected
// Matrix F = Matrix::Zero(6,3*12);
// F.block<2,12>(0,0) = Fs[0];
// F.block<2,12>(2,12) = Fs[1];
// F.block<2,12>(4,24) = Fs[2];
//
// std::cout << "E \n" << E << std::endl;
// std::cout << "P \n" << P << std::endl;
// std::cout << "F \n" << F << std::endl;
//
// Matrix Ft = F.transpose();
// Matrix H = Ft * F - Ft * E * P * Et * F;
// Vector v = Ft * (b - E * P * Et * b);
// Matrix expectedAugmentedHessian = Matrix::Zero(3*12+1, 3*12+1);
// expectedAugmentedHessian.block<36,36>(0,0) = H;
// expectedAugmentedHessian.block<36,1>(0,36) = v;
// expectedAugmentedHessian.block<1,36>(36,0) = v.transpose();
// expectedAugmentedHessian(36,36) = b.squaredNorm();
//
// EXPECT(assert_equal(expectedAugmentedHessian, actualAugmentedHessian));
// }
// { // SchurComplementAndRearrangeBlocks
// // Actual
// SymmetricBlockMatrix augmentedHessianBM = Set::SchurComplementAndRearrangeBlocks<3,12,6>(
// Fs,E,P,b,nonuniqueKeys,uniqueKeys);
// Matrix actualAugmentedHessian = augmentedHessianBM.selfadjointView();
//
// // Expected
// // we first need to build the Jacobian F according to unique keys
// Matrix F = Matrix::Zero(6,18);
// F.block<2,6>(0,0) = Fs[0].block<2,6>(0,0);
// F.block<2,6>(0,6) = Fs[0].block<2,6>(0,6);
// F.block<2,6>(2,6) = Fs[1].block<2,6>(0,0);
// F.block<2,6>(2,12) = Fs[1].block<2,6>(0,6);
// F.block<2,6>(4,12) = Fs[2].block<2,6>(0,0);
// F.block<2,6>(4,0) = Fs[2].block<2,6>(0,6);
//
// std::cout << "P \n" << P << std::endl;
// std::cout << "F \n" << F << std::endl;
//
// Matrix Ft = F.transpose();
// Matrix34 Et = E.transpose();
// Vector v = Ft * (b - E * P * Et * b);
// Matrix H = Ft * F - Ft * E * P * Et * F;
// Matrix expectedAugmentedHessian(19, 19);
// expectedAugmentedHessian << H, v, v.transpose(), b.squaredNorm();
//
// EXPECT(assert_equal(expectedAugmentedHessian, actualAugmentedHessian));
// }
}
/* ************************************************************************* */
#include <gtsam/geometry/StereoCamera.h>
TEST(CameraSet, Stereo) {