From 26f2b33c4717f89569963be3b4a794a438beccfa Mon Sep 17 00:00:00 2001 From: dellaert Date: Wed, 4 Mar 2015 23:01:47 -0800 Subject: [PATCH] Migrated to non-keyed Fblocks --- gtsam/slam/JacobianFactorSVD.h | 18 ++++++------ gtsam/slam/SmartFactorBase.h | 14 +++++----- gtsam/slam/SmartProjectionCameraFactor.h | 2 +- gtsam/slam/SmartProjectionFactor.h | 21 ++++++-------- .../tests/testSmartProjectionCameraFactor.cpp | 4 +-- .../tests/testSmartProjectionPoseFactor.cpp | 28 ++++++++++--------- 6 files changed, 44 insertions(+), 43 deletions(-) diff --git a/gtsam/slam/JacobianFactorSVD.h b/gtsam/slam/JacobianFactorSVD.h index 0b0d76fda..aac946901 100644 --- a/gtsam/slam/JacobianFactorSVD.h +++ b/gtsam/slam/JacobianFactorSVD.h @@ -16,7 +16,6 @@ class JacobianFactorSVD: public RegularJacobianFactor { typedef RegularJacobianFactor Base; typedef Eigen::Matrix MatrixZD; // e.g 2 x 6 with Z=Point2 - typedef std::pair KeyMatrixZD; typedef std::pair KeyMatrix; public: @@ -46,12 +45,13 @@ public: * * @Fblocks: */ - JacobianFactorSVD(const std::vector& Fblocks, - const Matrix& Enull, const Vector& b, // + JacobianFactorSVD(const FastVector& keys, + const std::vector& Fblocks, const Matrix& Enull, + const Vector& b, // const SharedDiagonal& model = SharedDiagonal()) : Base() { size_t numKeys = Enull.rows() / ZDim; - size_t j = 0, m2 = ZDim * numKeys - 3; + size_t m2 = ZDim * numKeys - 3; // PLAIN NULL SPACE TRICK // Matrix Q = Enull * Enull.transpose(); // BOOST_FOREACH(const KeyMatrixZD& it, Fblocks) @@ -59,11 +59,13 @@ public: // JacobianFactor factor(QF, Q * b); std::vector QF; QF.reserve(numKeys); - BOOST_FOREACH(const KeyMatrixZD& it, Fblocks) + for (size_t k = 0; k < Fblocks.size(); ++k) { + Key key = keys[k]; QF.push_back( - KeyMatrix(it.first, - (Enull.transpose()).block(0, ZDim * j++, m2, ZDim) * it.second)); - JacobianFactor::fillTerms(QF, - Enull.transpose() * b, model); + KeyMatrix(key, + (Enull.transpose()).block(0, ZDim * k, m2, ZDim) * Fblocks[k])); + } + JacobianFactor::fillTerms(QF, -Enull.transpose() * b, model); } }; diff --git a/gtsam/slam/SmartFactorBase.h b/gtsam/slam/SmartFactorBase.h index 947c81385..53a86abe0 100644 --- a/gtsam/slam/SmartFactorBase.h +++ b/gtsam/slam/SmartFactorBase.h @@ -281,12 +281,12 @@ public: * 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 * with respect to the point. The value of cameras/point are passed as parameters. + * TODO: Kill this obsolete method */ double computeJacobians(std::vector& Fblocks, Matrix& E, Vector& b, const Cameras& cameras, const Point3& point) const { // Project into Camera set and calculate derivatives - typename Cameras::FBlocks F; - b = reprojectionError(cameras, point, F, E); + b = reprojectionError(cameras, point, Fblocks, E); return b.squaredNorm(); } @@ -357,8 +357,8 @@ public: void whitenJacobians(std::vector& F, Matrix& E, Vector& b) const { noiseModel_->WhitenSystem(E, b); // TODO make WhitenInPlace work with any dense matrix type - BOOST_FOREACH(MatrixZD& Fblock,F) - Fblock.second = noiseModel_->Whiten(Fblock.second); + for (size_t i = 0; i < F.size(); i++) + F[i] = noiseModel_->Whiten(F[i]); } /** @@ -390,7 +390,7 @@ public: const size_t M = b.size(); Matrix3 P = PointCov(E, lambda, diagonalDamping); SharedIsotropic n = noiseModel::Isotropic::Sigma(M, noiseModel_->sigma()); - return boost::make_shared >(F, E, P, b, n); + return boost::make_shared >(keys_, F, E, P, b, n); } /** @@ -407,7 +407,7 @@ public: computeJacobiansSVD(F, E0, b, cameras, point); SharedIsotropic n = noiseModel::Isotropic::Sigma(M - 3, noiseModel_->sigma()); - return boost::make_shared >(F, E0, b, n); + return boost::make_shared >(keys_, F, E0, b, n); } /// Create BIG block-diagonal matrix F from Fblocks @@ -416,7 +416,7 @@ public: F.resize(ZDim * m, Dim * m); F.setZero(); for (size_t i = 0; i < m; ++i) - F.block(This::ZDim * i, Dim * i) = Fblocks.at(i).second; + F.block(ZDim * i, Dim * i) = Fblocks.at(i); } private: diff --git a/gtsam/slam/SmartProjectionCameraFactor.h b/gtsam/slam/SmartProjectionCameraFactor.h index 3afd04188..b2eeba3e1 100644 --- a/gtsam/slam/SmartProjectionCameraFactor.h +++ b/gtsam/slam/SmartProjectionCameraFactor.h @@ -112,7 +112,7 @@ public: } /// linearize returns a Hessianfactor that is an approximation of error(p) - virtual boost::shared_ptr > linearizeToImplicit( + virtual boost::shared_ptr > linearizeToImplicit( const Values& values, double lambda=0.0) const { return Base::createRegularImplicitSchurFactor(Base::cameras(values),lambda); } diff --git a/gtsam/slam/SmartProjectionFactor.h b/gtsam/slam/SmartProjectionFactor.h index 7ab26c0a1..5f9a6750a 100644 --- a/gtsam/slam/SmartProjectionFactor.h +++ b/gtsam/slam/SmartProjectionFactor.h @@ -278,7 +278,7 @@ public: Vector b; double f; { - std::vector Fblocks; + std::vector Fblocks; f = computeJacobiansWithTriangulatedPoint(Fblocks, E, b, cameras); Base::whitenJacobians(Fblocks, E, b); Base::FillDiagonalF(Fblocks, F); // expensive ! @@ -326,12 +326,12 @@ public: } // create factor - boost::shared_ptr > createRegularImplicitSchurFactor( + boost::shared_ptr > createRegularImplicitSchurFactor( const Cameras& cameras, double lambda) const { if (triangulateForLinearize(cameras)) return Base::createRegularImplicitSchurFactor(cameras, *result_, lambda); else - return boost::shared_ptr >(); + return boost::shared_ptr >(); } /// create factor @@ -374,7 +374,7 @@ public: /// Assumes the point has been computed /// Note E can be 2m*3 or 2m*2, in case point is degenerate double computeJacobiansWithTriangulatedPoint( - std::vector& Fblocks, Matrix& E, Vector& b, + std::vector& Fblocks, Matrix& E, Vector& b, const Cameras& cameras) const { if (!result_) { @@ -385,18 +385,15 @@ public: int m = this->keys_.size(); E = zeros(2 * m, 2); b = zero(2 * m); - double f = 0; for (size_t i = 0; i < this->measured_.size(); i++) { Matrix Fi, Ei; Vector bi = -(cameras[i].projectPointAtInfinity(*result_, Fi, Ei) - this->measured_.at(i)).vector(); - - f += bi.squaredNorm(); - Fblocks.push_back(typename Base::KeyMatrix2D(this->keys_[i], Fi)); + Fblocks.push_back(Fi); E.block<2, 2>(2 * i, 0) = Ei; subInsert(b, bi, 2 * i); } - return f; + return b.squaredNorm(); } else { // valid result: just return Base version return Base::computeJacobians(Fblocks, E, b, cameras, *result_); @@ -405,7 +402,7 @@ public: /// Version that takes values, and creates the point bool triangulateAndComputeJacobians( - std::vector& Fblocks, Matrix& E, Vector& b, + std::vector& Fblocks, Matrix& E, Vector& b, const Values& values) const { Cameras cameras = this->cameras(values); bool nonDegenerate = triangulateForLinearize(cameras); @@ -416,8 +413,8 @@ public: /// takes values bool triangulateAndComputeJacobiansSVD( - std::vector& Fblocks, Matrix& Enull, - Vector& b, const Values& values) const { + std::vector& Fblocks, Matrix& Enull, Vector& b, + const Values& values) const { Cameras cameras = this->cameras(values); bool nonDegenerate = triangulateForLinearize(cameras); if (nonDegenerate) diff --git a/gtsam/slam/tests/testSmartProjectionCameraFactor.cpp b/gtsam/slam/tests/testSmartProjectionCameraFactor.cpp index 56ff47c3e..c7cf0283f 100644 --- a/gtsam/slam/tests/testSmartProjectionCameraFactor.cpp +++ b/gtsam/slam/tests/testSmartProjectionCameraFactor.cpp @@ -772,7 +772,7 @@ TEST( SmartProjectionCameraFactor, computeImplicitJacobian ) { Point3 point; if (factor1->point()) point = *(factor1->point()); - vector Fblocks; + vector Fblocks; factor1->computeJacobians(Fblocks, expectedE, expectedb, cameras, point); NonlinearFactorGraph generalGraph; @@ -823,7 +823,7 @@ TEST( SmartProjectionCameraFactor, implicitJacobianFactor ) { implicitFactor->add(level_uv_right, c2, unit2); GaussianFactor::shared_ptr gaussianImplicitSchurFactor = implicitFactor->linearize(values); - typedef RegularImplicitSchurFactor<9> Implicit9; + typedef RegularImplicitSchurFactor Implicit9; Implicit9& implicitSchurFactor = dynamic_cast(*gaussianImplicitSchurFactor); diff --git a/gtsam/slam/tests/testSmartProjectionPoseFactor.cpp b/gtsam/slam/tests/testSmartProjectionPoseFactor.cpp index fcebd12a7..71d30b6d8 100644 --- a/gtsam/slam/tests/testSmartProjectionPoseFactor.cpp +++ b/gtsam/slam/tests/testSmartProjectionPoseFactor.cpp @@ -142,7 +142,7 @@ TEST_UNSAFE( SmartProjectionPoseFactor, noiseless ) { // Calculate using computeJacobians Vector b; - vector Fblocks; + vector Fblocks; double actualError3 = factor.computeJacobians(Fblocks, E, b, cameras, *point); EXPECT(assert_equal(expectedE, E, 1e-7)); EXPECT_DOUBLES_EQUAL(expectedError, actualError3, 1e-8); @@ -264,10 +264,12 @@ TEST( SmartProjectionPoseFactor, 3poses_smart_projection_factor ) { /* *************************************************************************/ TEST( SmartProjectionPoseFactor, Factors ) { + typedef PinholePose Camera; + // Default cameras for simple derivatives Rot3 R; static Cal3_S2::shared_ptr sharedK(new Cal3_S2(100, 100, 0, 0, 0)); - PinholePose cam1(Pose3(R, Point3(0, 0, 0)), sharedK), cam2( + Camera cam1(Pose3(R, Point3(0, 0, 0)), sharedK), cam2( Pose3(R, Point3(1, 0, 0)), sharedK); // one landmarks 1m in front of camera @@ -350,15 +352,19 @@ TEST( SmartProjectionPoseFactor, Factors ) { E(2, 0) = 10; E(2, 2) = 1; E(3, 1) = 10; - vector > Fblocks = list_of > // - (make_pair(x1, F1))(make_pair(x2, F2)); + vector Fblocks = list_of(F1)(F2); Vector b(4); b.setZero(); + // Create smart factors + FastVector keys; + keys.push_back(x1); + keys.push_back(x2); + // createJacobianQFactor SharedIsotropic n = noiseModel::Isotropic::Sigma(4, sigma); Matrix3 P = (E.transpose() * E).inverse(); - JacobianFactorQ<6, 2> expectedQ(Fblocks, E, P, b, n); + JacobianFactorQ<6, 2> expectedQ(keys, Fblocks, E, P, b, n); EXPECT(assert_equal(expectedInformation, expectedQ.information(), 1e-8)); boost::shared_ptr > actualQ = @@ -370,13 +376,13 @@ TEST( SmartProjectionPoseFactor, Factors ) { // Whiten for RegularImplicitSchurFactor (does not have noise model) model->WhitenSystem(E, b); Matrix3 whiteP = (E.transpose() * E).inverse(); - BOOST_FOREACH(SmartFactor::KeyMatrix2D& Fblock,Fblocks) - Fblock.second = model->Whiten(Fblock.second); + Fblocks[0] = model->Whiten(Fblocks[0]); + Fblocks[1] = model->Whiten(Fblocks[1]); // createRegularImplicitSchurFactor - RegularImplicitSchurFactor<6> expected(Fblocks, E, whiteP, b); + RegularImplicitSchurFactor expected(keys, Fblocks, E, whiteP, b); - boost::shared_ptr > actual = + boost::shared_ptr > actual = smartFactor1->createRegularImplicitSchurFactor(cameras, 0.0); CHECK(actual); EXPECT(assert_equal(expectedInformation, expected.information(), 1e-8)); @@ -764,8 +770,6 @@ TEST( SmartProjectionPoseFactor, 3poses_projection_factor ) { values.insert(L(1), landmark1); values.insert(L(2), landmark2); values.insert(L(3), landmark3); - if (isDebugTest) - values.at(x3).print("Pose3 before optimization: "); DOUBLES_EQUAL(48406055, graph.error(values), 1); @@ -779,8 +783,6 @@ TEST( SmartProjectionPoseFactor, 3poses_projection_factor ) { DOUBLES_EQUAL(0, graph.error(result), 1e-9); - if (isDebugTest) - result.at(x3).print("Pose3 after optimization: "); EXPECT(assert_equal(pose_above, result.at(x3), 1e-7)); }