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

fixed last test - this is good to go!

release/4.3a0
lcarlone 2021-07-24 00:47:17 -04:00
parent 9834042040
commit 2f03e588fc
1 changed files with 75 additions and 77 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -17,6 +17,7 @@
*/ */
#include <gtsam/geometry/CameraSet.h> #include <gtsam/geometry/CameraSet.h>
#include <gtsam/geometry/Cal3_S2.h>
#include <gtsam/geometry/Pose3.h> #include <gtsam/geometry/Pose3.h>
#include <gtsam/base/numericalDerivative.h> #include <gtsam/base/numericalDerivative.h>
#include <CppUnitLite/TestHarness.h> #include <CppUnitLite/TestHarness.h>
@ -127,10 +128,48 @@ TEST(CameraSet, Pinhole) {
/* ************************************************************************* */ /* ************************************************************************* */
TEST(CameraSet, SchurComplementAndRearrangeBlocks) { TEST(CameraSet, SchurComplementAndRearrangeBlocks) {
typedef PinholePose<Cal3Bundler> Camera; typedef PinholePose<Cal3_S2> Camera;
typedef CameraSet<Camera> Set; typedef CameraSet<Camera> Set;
typedef Point2Vector ZZ;
// 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 = 4 * Matrix::Identity(6, 3) + Matrix::Ones(6, 3);
E(0, 0) = 3;
E(1, 1) = 2;
E(2, 2) = 5;
Matrix 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) = 1 * Matrix::Ones(2, 12);
F.block<2, 12>(2, 12) = 2 * Matrix::Ones(2, 12);
F.block<2, 12>(4, 24) = 3 * Matrix::Ones(2, 12);
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
KeyVector nonuniqueKeys; KeyVector nonuniqueKeys;
nonuniqueKeys.push_back(0); nonuniqueKeys.push_back(0);
nonuniqueKeys.push_back(1); nonuniqueKeys.push_back(1);
@ -144,72 +183,31 @@ TEST(CameraSet, SchurComplementAndRearrangeBlocks) {
uniqueKeys.push_back(1); uniqueKeys.push_back(1);
uniqueKeys.push_back(2); uniqueKeys.push_back(2);
// this is the (block) Jacobian with respect to the nonuniqueKeys // Actual
std::vector<Eigen::Matrix<double,2, 12>, SymmetricBlockMatrix augmentedHessianBM =
Eigen::aligned_allocator<Eigen::Matrix<double,2,12> > > Fs; Set::SchurComplementAndRearrangeBlocks<3, 12, 6>(Fs, E, P, b,
Fs.push_back(1 * Matrix::Ones(2,12)); // corresponding to key pair (0,1) nonuniqueKeys,
Fs.push_back(2 * Matrix::Ones(2,12)); // corresponding to key pair (1,2) uniqueKeys);
Fs.push_back(3 * Matrix::Ones(2,12)); // corresponding to key pair (2,0) Matrix actualAugmentedHessian = augmentedHessianBM.selfadjointView();
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 // Expected
// // Actual // we first need to build the Jacobian F according to unique keys
// SymmetricBlockMatrix augmentedHessianBM = Set::SchurComplement<3,12>(Fs,E,P,b); Matrix F = Matrix::Zero(6, 18);
// Matrix actualAugmentedHessian = augmentedHessianBM.selfadjointView(); 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);
// // Expected F.block<2, 6>(2, 6) = Fs[1].block<2, 6>(0, 0);
// Matrix F = Matrix::Zero(6,3*12); F.block<2, 6>(2, 12) = Fs[1].block<2, 6>(0, 6);
// F.block<2,12>(0,0) = Fs[0]; F.block<2, 6>(4, 12) = Fs[2].block<2, 6>(0, 0);
// F.block<2,12>(2,12) = Fs[1]; F.block<2, 6>(4, 0) = Fs[2].block<2, 6>(0, 6);
// 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 Matrix Ft = F.transpose();
// // Actual Vector v = Ft * (b - E * P * Et * b);
// SymmetricBlockMatrix augmentedHessianBM = Set::SchurComplementAndRearrangeBlocks<3,12,6>( Matrix H = Ft * F - Ft * E * P * Et * F;
// Fs,E,P,b,nonuniqueKeys,uniqueKeys); Matrix expectedAugmentedHessian(19, 19);
// Matrix actualAugmentedHessian = augmentedHessianBM.selfadjointView(); expectedAugmentedHessian << H, v, v.transpose(), b.squaredNorm();
//
// // Expected EXPECT(assert_equal(expectedAugmentedHessian, actualAugmentedHessian));
// // 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));
// }
} }
/* ************************************************************************* */ /* ************************************************************************* */