diff --git a/.cproject b/.cproject index 46b623bb9..e08e9bc62 100644 --- a/.cproject +++ b/.cproject @@ -582,6 +582,7 @@ make + tests/testBayesTree.run true false @@ -589,6 +590,7 @@ make + testBinaryBayesNet.run true false @@ -636,6 +638,7 @@ make + testSymbolicBayesNet.run true false @@ -643,6 +646,7 @@ make + tests/testSymbolicFactor.run true false @@ -650,6 +654,7 @@ make + testSymbolicFactorGraph.run true false @@ -665,6 +670,7 @@ make + tests/testBayesTree true false @@ -1000,6 +1006,7 @@ make + testErrors.run true false @@ -1229,6 +1236,46 @@ true true + + make + -j5 + testBTree.run + true + true + true + + + make + -j5 + testDSF.run + true + true + true + + + make + -j5 + testDSFMap.run + true + true + true + + + make + -j5 + testDSFVector.run + true + true + true + + + make + -j5 + testFixedVector.run + true + true + true + make -j2 @@ -1311,7 +1358,6 @@ make - testSimulated2DOriented.run true false @@ -1351,7 +1397,6 @@ make - testSimulated2D.run true false @@ -1359,7 +1404,6 @@ make - testSimulated3D.run true false @@ -1373,46 +1417,6 @@ true true - - make - -j5 - testBTree.run - true - true - true - - - make - -j5 - testDSF.run - true - true - true - - - make - -j5 - testDSFMap.run - true - true - true - - - make - -j5 - testDSFVector.run - true - true - true - - - make - -j5 - testFixedVector.run - true - true - true - make -j5 @@ -1670,6 +1674,7 @@ cpack + -G DEB true false @@ -1677,6 +1682,7 @@ cpack + -G RPM true false @@ -1684,6 +1690,7 @@ cpack + -G TGZ true false @@ -1691,6 +1698,7 @@ cpack + --config CPackSourceConfig.cmake true false @@ -2417,6 +2425,7 @@ make + testGraph.run true false @@ -2424,6 +2433,7 @@ make + testJunctionTree.run true false @@ -2431,6 +2441,7 @@ make + testSymbolicBayesNetB.run true false @@ -2636,6 +2647,14 @@ true true + + make + -j5 + testImplicitSchurFactor.run + true + true + true + make -j2 @@ -2910,7 +2929,6 @@ make - tests/testGaussianISAM2 true false diff --git a/gtsam/slam/ImplicitSchurFactor.h b/gtsam/slam/ImplicitSchurFactor.h index c0f339b30..8c6d8f1ff 100644 --- a/gtsam/slam/ImplicitSchurFactor.h +++ b/gtsam/slam/ImplicitSchurFactor.h @@ -81,7 +81,7 @@ public: } /// Get matrix P - inline const Matrix& getPointCovariance() const { + inline const Matrix3& getPointCovariance() const { return PointCovariance_; } @@ -286,26 +286,27 @@ public: return 0.5 * (result + f); } - /// needed to be GaussianFactor - (I - E*P*E')*(F*x - b) - // This is wrong and does not match the definition in Hessian - // virtual double error(const VectorValues& x) const { - // - // // resize does not do malloc if correct size - // e1.resize(size()); - // e2.resize(size()); - // - // // e1 = F * x - b = (2m*dm)*dm - // for (size_t k = 0; k < size(); ++k) - // e1[k] = Fblocks_[k].second * x.at(keys_[k]) - b_.segment < 2 > (k * 2); - // projectError(e1, e2); - // - // double result = 0; - // for (size_t k = 0; k < size(); ++k) - // result += dot(e2[k], e2[k]); - // - // std::cout << "implicitFactor::error result " << result << std::endl; - // return 0.5 * result; - // } + // needed to be GaussianFactor - (I - E*P*E')*(F*x - b) + // This is wrong and does not match the definition in Hessian, + // but it matches the definition of the Jacobian factor (JF) + double errorJF(const VectorValues& x) const { + + // resize does not do malloc if correct size + e1.resize(size()); + e2.resize(size()); + + // e1 = F * x - b = (2m*dm)*dm + for (size_t k = 0; k < size(); ++k) + e1[k] = Fblocks_[k].second * x.at(keys_[k]) - b_.segment < 2 > (k * 2); + projectError(e1, e2); + + double result = 0; + for (size_t k = 0; k < size(); ++k) + result += dot(e2[k], e2[k]); + + // std::cout << "implicitFactor::error result " << result << std::endl; + return 0.5 * result; + } /** * @brief Calculate corrected error Q*e = (I - E*P*E')*e */ diff --git a/gtsam/slam/tests/testImplicitSchurFactor.cpp b/gtsam/slam/tests/testImplicitSchurFactor.cpp new file mode 100644 index 000000000..77faaacc1 --- /dev/null +++ b/gtsam/slam/tests/testImplicitSchurFactor.cpp @@ -0,0 +1,259 @@ +/** + * @file testImplicitSchurFactor.cpp + * @brief unit test implicit jacobian factors + * @author Frank Dellaert + * @date Oct 20, 2013 + */ + +//#include +#include +//#include +#include +//#include "gtsam_unstable/slam/JacobianFactorQR.h" +#include "gtsam/slam/JacobianFactorQR.h" + +#include +#include +#include +#include + +#include +#include +#include +#include +#include + +using namespace std; +using namespace boost::assign; +using namespace gtsam; + +// F +typedef Eigen::Matrix Matrix26; +const Matrix26 F0 = Matrix26::Ones(); +const Matrix26 F1 = 2 * Matrix26::Ones(); +const Matrix26 F3 = 3 * Matrix26::Ones(); +const vector > Fblocks = list_of > // + (make_pair(0, F0))(make_pair(1, F1))(make_pair(3, F3)); +// RHS and sigmas +const Vector b = (Vector(6) << 1., 2., 3., 4., 5., 6.); + +//************************************************************************************* +TEST( implicitSchurFactor, creation ) { + // Matrix E = Matrix::Ones(6,3); + Matrix E = zeros(6, 3); + E.block<2,2>(0, 0) = eye(2); + E.block<2,3>(2, 0) = 2 * ones(2, 3); + Matrix3 P = (E.transpose() * E).inverse(); + ImplicitSchurFactor<6> expected(Fblocks, E, P, b); + Matrix expectedP = expected.getPointCovariance(); + EXPECT(assert_equal(expectedP, P)); +} + +/* ************************************************************************* */ +TEST( implicitSchurFactor, addHessianMultiply ) { + + Matrix E = zeros(6, 3); + E.block<2,2>(0, 0) = eye(2); + E.block<2,3>(2, 0) = 2 * ones(2, 3); + E.block<2,2>(4, 1) = eye(2); + Matrix3 P = (E.transpose() * E).inverse(); + + double alpha = 0.5; + VectorValues xvalues = map_list_of // + (0, gtsam::repeat(6, 2))// + (1, gtsam::repeat(6, 4))// + (2, gtsam::repeat(6, 0))// distractor + (3, gtsam::repeat(6, 8)); + + VectorValues yExpected = map_list_of// + (0, gtsam::repeat(6, 27))// + (1, gtsam::repeat(6, -40))// + (2, gtsam::repeat(6, 0))// distractor + (3, gtsam::repeat(6, 279)); + + // Create full F + size_t M=4, m = 3, d = 6; + Matrix F(2 * m, d * M); + F << F0, zeros(2, d * 3), zeros(2, d), F1, zeros(2, d*2), zeros(2, d * 3), F3; + + // Calculate expected result F'*alpha*(I - E*P*E')*F*x + FastVector keys; + keys += 0,1,2,3; + Vector x = xvalues.vector(keys); + Vector expected = zero(24); + ImplicitSchurFactor<6>::multiplyHessianAdd(F, E, P, alpha, x, expected); + EXPECT(assert_equal(expected, yExpected.vector(keys), 1e-8)); + + // Create ImplicitSchurFactor + ImplicitSchurFactor<6> implicitFactor(Fblocks, E, P, b); + + VectorValues zero = 0 * yExpected;// quick way to get zero w right structure + { // First Version + VectorValues yActual = zero; + implicitFactor.multiplyHessianAdd(alpha, xvalues, yActual); + EXPECT(assert_equal(yExpected, yActual, 1e-8)); + implicitFactor.multiplyHessianAdd(alpha, xvalues, yActual); + EXPECT(assert_equal(2 * yExpected, yActual, 1e-8)); + implicitFactor.multiplyHessianAdd(-1, xvalues, yActual); + EXPECT(assert_equal(zero, yActual, 1e-8)); + } + + typedef Eigen::Matrix DeltaX; + typedef Eigen::Map XMap; + double* y = new double[24]; + double* xdata = x.data(); + + { // Raw memory Version + std::fill(y, y + 24, 0);// zero y ! + implicitFactor.multiplyHessianAdd(alpha, xdata, y); + EXPECT(assert_equal(expected, XMap(y), 1e-8)); + implicitFactor.multiplyHessianAdd(alpha, xdata, y); + EXPECT(assert_equal(Vector(2 * expected), XMap(y), 1e-8)); + implicitFactor.multiplyHessianAdd(-1, xdata, y); + EXPECT(assert_equal(Vector(0 * expected), XMap(y), 1e-8)); + } + + // Create JacobianFactor with same error + const SharedDiagonal model; + JacobianFactorQ<6> jf(Fblocks, E, P, b, model); + + { // error + double expectedError = jf.error(xvalues); + double actualError = implicitFactor.errorJF(xvalues); + DOUBLES_EQUAL(expectedError,actualError,1e-7) + } + + { // JacobianFactor with same error + VectorValues yActual = zero; + jf.multiplyHessianAdd(alpha, xvalues, yActual); + EXPECT(assert_equal(yExpected, yActual, 1e-8)); + jf.multiplyHessianAdd(alpha, xvalues, yActual); + EXPECT(assert_equal(2 * yExpected, yActual, 1e-8)); + jf.multiplyHessianAdd(-1, xvalues, yActual); + EXPECT(assert_equal(zero, yActual, 1e-8)); + } + + { // check hessian Diagonal + VectorValues diagExpected = jf.hessianDiagonal(); + VectorValues diagActual = implicitFactor.hessianDiagonal(); + EXPECT(assert_equal(diagExpected, diagActual, 1e-8)); + } + + { // check hessian Block Diagonal + map BD = jf.hessianBlockDiagonal(); + map actualBD = implicitFactor.hessianBlockDiagonal(); + LONGS_EQUAL(3,actualBD.size()); + EXPECT(assert_equal(BD[0],actualBD[0])); + EXPECT(assert_equal(BD[1],actualBD[1])); + EXPECT(assert_equal(BD[3],actualBD[3])); + } + + { // Raw memory Version + std::fill(y, y + 24, 0);// zero y ! + jf.multiplyHessianAdd(alpha, xdata, y); + EXPECT(assert_equal(expected, XMap(y), 1e-8)); + jf.multiplyHessianAdd(alpha, xdata, y); + EXPECT(assert_equal(Vector(2 * expected), XMap(y), 1e-8)); + jf.multiplyHessianAdd(-1, xdata, y); + EXPECT(assert_equal(Vector(0 * expected), XMap(y), 1e-8)); + } + + { // Check gradientAtZero + VectorValues expected = jf.gradientAtZero(); + VectorValues actual = implicitFactor.gradientAtZero(); + EXPECT(assert_equal(expected, actual, 1e-8)); + } + + // Create JacobianFactorQR + JacobianFactorQR<6> jfq(Fblocks, E, P, b, model); + { + const SharedDiagonal model; + VectorValues yActual = zero; + jfq.multiplyHessianAdd(alpha, xvalues, yActual); + EXPECT(assert_equal(yExpected, yActual, 1e-8)); + jfq.multiplyHessianAdd(alpha, xvalues, yActual); + EXPECT(assert_equal(2 * yExpected, yActual, 1e-8)); + jfq.multiplyHessianAdd(-1, xvalues, yActual); + EXPECT(assert_equal(zero, yActual, 1e-8)); + } + + { // Raw memory Version + std::fill(y, y + 24, 0);// zero y ! + jfq.multiplyHessianAdd(alpha, xdata, y); + EXPECT(assert_equal(expected, XMap(y), 1e-8)); + jfq.multiplyHessianAdd(alpha, xdata, y); + EXPECT(assert_equal(Vector(2 * expected), XMap(y), 1e-8)); + jfq.multiplyHessianAdd(-1, xdata, y); + EXPECT(assert_equal(Vector(0 * expected), XMap(y), 1e-8)); + } + delete [] y; +} + +/* ************************************************************************* */ +TEST(implicitSchurFactor, hessianDiagonal) +{ + /* TESTED AGAINST MATLAB + * F = [ones(2,6) zeros(2,6) zeros(2,6) + zeros(2,6) 2*ones(2,6) zeros(2,6) + zeros(2,6) zeros(2,6) 3*ones(2,6)] + E = [[1:6] [1:6] [0.5 1:5]]; + E = reshape(E',3,6)' + P = inv(E' * E) + H = F' * (eye(6) - E * P * E') * F + diag(H) + */ + Matrix E(6,3); + E.block<2,3>(0, 0) << 1,2,3,4,5,6; + E.block<2,3>(2, 0) << 1,2,3,4,5,6; + E.block<2,3>(4, 0) << 0.5,1,2,3,4,5; + Matrix3 P = (E.transpose() * E).inverse(); + ImplicitSchurFactor<6> factor(Fblocks, E, P, b); + + // hessianDiagonal + VectorValues expected; + expected.insert(0, 1.195652*ones(6)); + expected.insert(1, 4.782608*ones(6)); + expected.insert(3, 7.043478*ones(6)); + EXPECT(assert_equal(expected, factor.hessianDiagonal(),1e-5)); + + // hessianBlockDiagonal + map actualBD = factor.hessianBlockDiagonal(); + LONGS_EQUAL(3,actualBD.size()); + Matrix FtE0 = F0.transpose() * E.block<2,3>(0, 0); + Matrix FtE1 = F1.transpose() * E.block<2,3>(2, 0); + Matrix FtE3 = F3.transpose() * E.block<2,3>(4, 0); + + // variant one + EXPECT(assert_equal(F0.transpose()*F0-FtE0*P*FtE0.transpose(),actualBD[0])); + EXPECT(assert_equal(F1.transpose()*F1-FtE1*P*FtE1.transpose(),actualBD[1])); + EXPECT(assert_equal(F3.transpose()*F3-FtE3*P*FtE3.transpose(),actualBD[3])); + + // variant two + Matrix I2 = eye(2); + Matrix E0 = E.block<2,3>(0, 0); + Matrix F0t = F0.transpose(); + EXPECT(assert_equal(F0t*F0-F0t*E0*P*E0.transpose()*F0,actualBD[0])); + EXPECT(assert_equal(F0t*(F0-E0*P*E0.transpose()*F0),actualBD[0])); + + Matrix M1 = F0t*(F0-E0*P*E0.transpose()*F0); + Matrix M2 = F0t*F0-F0t*E0*P*E0.transpose()*F0; + + EXPECT(assert_equal( M1 , actualBD[0] )); + EXPECT(assert_equal( M1 , M2 )); + + Matrix M1b = F0t*(E0*P*E0.transpose()*F0); + Matrix M2b = F0t*E0*P*E0.transpose()*F0; + EXPECT(assert_equal( M1b , M2b )); + + EXPECT(assert_equal(F0t*(I2-E0*P*E0.transpose())*F0,actualBD[0])); + EXPECT(assert_equal(F1.transpose()*F1-FtE1*P*FtE1.transpose(),actualBD[1])); + EXPECT(assert_equal(F3.transpose()*F3-FtE3*P*FtE3.transpose(),actualBD[3])); +} + +/* ************************************************************************* */ +int main(void) { + TestResult tr; + int result = TestRegistry::runAllTests(tr); + return result; +} +//*************************************************************************************