From fd71974ff3250b12913ef805b4f0c8c4ac48f152 Mon Sep 17 00:00:00 2001 From: dellaert Date: Wed, 25 Feb 2015 20:52:16 +0100 Subject: [PATCH] Got mostly rid of computeEP: just a call to cameras.project2 --- gtsam/slam/SmartFactorBase.h | 22 ++++++----------- gtsam/slam/SmartProjectionFactor.h | 9 ++++--- .../tests/testSmartProjectionPoseFactor.cpp | 24 ++++++++++--------- .../slam/SmartStereoProjectionFactor.h | 14 +++++------ 4 files changed, 32 insertions(+), 37 deletions(-) diff --git a/gtsam/slam/SmartFactorBase.h b/gtsam/slam/SmartFactorBase.h index d33b5957f..a5d2cfabe 100644 --- a/gtsam/slam/SmartFactorBase.h +++ b/gtsam/slam/SmartFactorBase.h @@ -306,13 +306,6 @@ public: return (EtE).inverse(); } - /// Assumes non-degenerate ! - void computeEP(Matrix& E, Matrix& P, const Cameras& cameras, - const Point3& point) const { - cameras.reprojectionError(point, measured_, boost::none, E); - P = PointCov(E); - } - /** * Compute F, E, and b (called below in both vanilla and SVD versions), where * F is a vector of derivatives wrpt the cameras, and E the stacked derivatives @@ -394,8 +387,7 @@ public: const Cameras& cameras, const Point3& point, const double lambda = 0.0, bool diagonalDamping = false) const { - int numKeys = this->keys_.size(); - + int m = this->keys_.size(); std::vector Fblocks; Matrix E; Vector b; @@ -405,8 +397,8 @@ public: //#define HESSIAN_BLOCKS // slower, as internally the Hessian factor will transform the blocks into SymmetricBlockMatrix #ifdef HESSIAN_BLOCKS // Create structures for Hessian Factors - std::vector < Matrix > Gs(numKeys * (numKeys + 1) / 2); - std::vector < Vector > gs(numKeys); + std::vector < Matrix > Gs(m * (m + 1) / 2); + std::vector < Vector > gs(m); sparseSchurComplement(Fblocks, E, P, b, Gs, gs); // schurComplement(Fblocks, E, P, b, Gs, gs); @@ -416,15 +408,15 @@ public: return boost::make_shared < RegularHessianFactor > (this->keys_, Gs, gs, f); #else // we create directly a SymmetricBlockMatrix - size_t n1 = Dim * numKeys + 1; - std::vector dims(numKeys + 1); // this also includes the b term + size_t n1 = Dim * m + 1; + std::vector dims(m + 1); // this also includes the b term std::fill(dims.begin(), dims.end() - 1, Dim); dims.back() = 1; SymmetricBlockMatrix augmentedHessian(dims, Matrix::Zero(n1, n1)); // for 10 cameras, size should be (10*Dim+1 x 10*Dim+1) sparseSchurComplement(Fblocks, E, P, b, augmentedHessian); // augmentedHessian.matrix().block (i1,i2) = ... - augmentedHessian(numKeys, numKeys)(0, 0) = f; - return boost::make_shared >(this->keys_, + augmentedHessian(m, m)(0, 0) = f; + return boost::make_shared >(keys_, augmentedHessian); #endif } diff --git a/gtsam/slam/SmartProjectionFactor.h b/gtsam/slam/SmartProjectionFactor.h index d932a1672..b4fad72fb 100644 --- a/gtsam/slam/SmartProjectionFactor.h +++ b/gtsam/slam/SmartProjectionFactor.h @@ -482,12 +482,15 @@ public: return true; } - /// Takes values - bool computeEP(Matrix& E, Matrix& P, const Values& values) const { + /** + * Triangulate and compute derivative of error with respect to point + * @return whether triangulation worked + */ + bool triangulateAndComputeE(Matrix& E, const Values& values) const { Cameras cameras; bool nonDegenerate = computeCamerasAndTriangulate(values, cameras); if (nonDegenerate) - Base::computeEP(E, P, cameras, point_); + cameras.project2(point_, boost::none, E); return nonDegenerate; } diff --git a/gtsam/slam/tests/testSmartProjectionPoseFactor.cpp b/gtsam/slam/tests/testSmartProjectionPoseFactor.cpp index 78b8b5240..caa0e5162 100644 --- a/gtsam/slam/tests/testSmartProjectionPoseFactor.cpp +++ b/gtsam/slam/tests/testSmartProjectionPoseFactor.cpp @@ -131,18 +131,20 @@ TEST_UNSAFE( SmartProjectionPoseFactor, noiseless ) { // Calculate expected derivative for point (easiest to check) boost::function f = // boost::bind(&SmartFactor::whitenedErrors, factor, cameras, _1); + + // Calculate using computeEP + Matrix actualE; + factor.triangulateAndComputeE(actualE, values); + + // get point boost::optional point = factor.point(); CHECK(point); - // Note ! After refactor the noiseModel is only in the factors, not these matrices + // calculate numerical derivative with triangulated point Matrix expectedE = sigma * numericalDerivative11(f, *point); - - // Calculate using computeEP - Matrix actualE, PointCov; - factor.computeEP(actualE, PointCov, values); EXPECT(assert_equal(expectedE, actualE, 1e-7)); - // Calculate using reprojectionError, note not yet divided by sigma ! + // Calculate using reprojectionError SmartFactor::Cameras::FBlocks F; Matrix E; Vector actualErrors = factor.reprojectionError(cameras, *point, F, E); @@ -361,6 +363,10 @@ TEST( SmartProjectionPoseFactor, smartFactorWithSensorBodyTransform ) { // Check derivatives + // Calculate E and P using computeEP, triangulates + Matrix actualE; + smartFactor1->triangulateAndComputeE(actualE, values); + // Calculate expected derivative for point (easiest to check) SmartFactor::Cameras cameras = smartFactor1->cameras(values); boost::function f = // @@ -370,10 +376,6 @@ TEST( SmartProjectionPoseFactor, smartFactorWithSensorBodyTransform ) { // TODO, this is really a test of CameraSet Matrix expectedE = numericalDerivative11(f, *point); - - // Calculate using computeEP - Matrix actualE, PointCov; - smartFactor1->computeEP(actualE, PointCov, values); EXPECT(assert_equal(expectedE, actualE, 1e-7)); // Calculate using reprojectionError @@ -383,7 +385,7 @@ TEST( SmartProjectionPoseFactor, smartFactorWithSensorBodyTransform ) { EXPECT(assert_equal(expectedE, E, 1e-7)); // Success ! The derivatives of reprojectionError now agree with f ! - EXPECT(assert_equal(f(*point) * sigma, actualErrors, 1e-7)); + EXPECT(assert_equal(f(*point), actualErrors, 1e-7)); } /* *************************************************************************/ diff --git a/gtsam_unstable/slam/SmartStereoProjectionFactor.h b/gtsam_unstable/slam/SmartStereoProjectionFactor.h index 5c38ccca8..551ad078c 100644 --- a/gtsam_unstable/slam/SmartStereoProjectionFactor.h +++ b/gtsam_unstable/slam/SmartStereoProjectionFactor.h @@ -513,17 +513,15 @@ public: return true; } - /// Assumes non-degenerate ! - void computeEP(Matrix& E, Matrix& PointCov, const Cameras& cameras) const { - Base::computeEP(E, PointCov, cameras, point_); - } - - /// Takes values - bool computeEP(Matrix& E, Matrix& PointCov, const Values& values) const { + /** + * Triangulate and compute derivative of error with respect to point + * @return whether triangulation worked + */ + bool triangulateAndComputeE(Matrix& E, const Values& values) const { Cameras cameras; bool nonDegenerate = computeCamerasAndTriangulate(values, cameras); if (nonDegenerate) - computeEP(E, PointCov, cameras); + cameras.project2(point_, boost::none, E); return nonDegenerate; }