diff --git a/gtsam/slam/JacobianFactorSVD.h b/gtsam/slam/JacobianFactorSVD.h index 255c799a6..e5e39d1a1 100644 --- a/gtsam/slam/JacobianFactorSVD.h +++ b/gtsam/slam/JacobianFactorSVD.h @@ -9,7 +9,7 @@ namespace gtsam { /** - * JacobianFactor for Schur complement that uses Q noise model + * JacobianFactor for Schur complement */ template class JacobianFactorSVD: public RegularJacobianFactor { @@ -38,7 +38,14 @@ public: JacobianFactor::fillTerms(QF, zeroVector, model); } - /// Constructor + /** + * @brief Constructor + * Takes the CameraSet derivatives (as ZDim*D blocks of block-diagonal F) + * and a reduced point derivative, Enull + * and creates a reduced-rank Jacobian factor on the CameraSet + * + * @Fblocks: + */ JacobianFactorSVD(const std::vector& Fblocks, const Matrix& Enull, const Vector& b, // const SharedDiagonal& model = SharedDiagonal()) : diff --git a/gtsam/slam/SmartFactorBase.h b/gtsam/slam/SmartFactorBase.h index 3d29b34e8..d33b5957f 100644 --- a/gtsam/slam/SmartFactorBase.h +++ b/gtsam/slam/SmartFactorBase.h @@ -81,6 +81,7 @@ protected: boost::optional body_P_sensor_; // check that noise model is isotropic and the same + // TODO, this is hacky, we should just do this via constructor, not add void maybeSetNoiseModel(const SharedNoiseModel& sharedNoiseModel) { if (!sharedNoiseModel) return; @@ -213,6 +214,36 @@ public: && body_P_sensor_->equals(*e->body_P_sensor_))); } + /// Compute reprojection errors + Vector reprojectionError(const Cameras& cameras, const Point3& point) const { + return cameras.reprojectionError(point, measured_); + } + + /** + * Compute reprojection errors and derivatives + * TODO: the treatment of body_P_sensor_ is weird: the transformation + * is applied in the caller, but the derivatives are computed here. + */ + Vector reprojectionError(const Cameras& cameras, const Point3& point, + typename Cameras::FBlocks& F, Matrix& E) const { + + Vector b = cameras.reprojectionError(point, measured_, F, E); + + // Apply sensor chain rule if needed TODO: no simpler way ?? + if (body_P_sensor_) { + size_t m = keys_.size(); + for (size_t i = 0; i < m; i++) { + const Pose3& pose_i = cameras[i].pose(); + Pose3 w_Pose_body = pose_i.compose(body_P_sensor_->inverse()); + Matrix66 J; + Pose3 world_P_body = w_Pose_body.compose(*body_P_sensor_, J); + F[i].leftCols(6) *= J; + } + } + + return b; + } + /// Calculate vector of re-projection errors, noise model applied Vector whitenedErrors(const Cameras& cameras, const Point3& point) const { Vector b = cameras.reprojectionError(point, measured_); @@ -251,36 +282,6 @@ public: return 0.5 * b.dot(b); } - /// Compute reprojection errors - Vector reprojectionError(const Cameras& cameras, const Point3& point) const { - return cameras.reprojectionError(point, measured_); - } - - /** - * Compute reprojection errors and derivatives - * TODO: the treatment of body_P_sensor_ is weird: the transformation - * is applied in the caller, but the derivatives are computed here. - */ - Vector reprojectionError(const Cameras& cameras, const Point3& point, - typename Cameras::FBlocks& F, Matrix& E) const { - - Vector b = cameras.reprojectionError(point, measured_, F, E); - - // Apply sensor chain rule if needed TODO: no simpler way ?? - if (body_P_sensor_) { - size_t m = keys_.size(); - for (size_t i = 0; i < m; i++) { - const Pose3& pose_i = cameras[i].pose(); - Pose3 w_Pose_body = pose_i.compose(body_P_sensor_->inverse()); - Matrix66 J; - Pose3 world_P_body = w_Pose_body.compose(*body_P_sensor_, J); - F[i].leftCols(6) *= J; - } - } - - return b; - } - /// Computes Point Covariance P from E static Matrix3 PointCov(Matrix& E) { return (E.transpose() * E).inverse(); @@ -371,7 +372,6 @@ public: Eigen::JacobiSVD svd(E, Eigen::ComputeFullU); Vector s = svd.singularValues(); size_t m = this->keys_.size(); - // Enull = zeros(ZDim * m, ZDim * m - 3); Enull = svd.matrixU().block(0, 3, ZDim * m, ZDim * m - 3); // last ZDim*m-3 columns return f; @@ -661,7 +661,7 @@ public: Matrix E; Vector b; computeJacobians(F, E, b, cameras, point); - noiseModel_->WhitenSystem(E,b); + noiseModel_->WhitenSystem(E, b); Matrix3 P = PointCov(E, lambda, diagonalDamping); // TODO make WhitenInPlace work with any dense matrix type BOOST_FOREACH(KeyMatrix2D& Fblock,F) @@ -675,13 +675,15 @@ public: boost::shared_ptr > createJacobianQFactor( const Cameras& cameras, const Point3& point, double lambda = 0.0, bool diagonalDamping = false) const { - std::vector Fblocks; + std::vector F; Matrix E; Vector b; - computeJacobians(Fblocks, E, b, cameras, point); + computeJacobians(F, E, b, cameras, point); + const size_t M = b.size(); + std::cout << M << std::endl; Matrix3 P = PointCov(E, lambda, diagonalDamping); - return boost::make_shared > // - (Fblocks, E, P, b, noiseModel_); + SharedIsotropic n = noiseModel::Isotropic::Sigma(M, noiseModel_->sigma()); + return boost::make_shared >(F, E, P, b, n); } /** @@ -690,13 +692,15 @@ public: */ boost::shared_ptr createJacobianSVDFactor( const Cameras& cameras, const Point3& point, double lambda = 0.0) const { - size_t numKeys = this->keys_.size(); - std::vector Fblocks; + size_t m = this->keys_.size(); + std::vector F; Vector b; - Matrix Enull(ZDim * numKeys, ZDim * numKeys - 3); - computeJacobiansSVD(Fblocks, Enull, b, cameras, point); - return boost::make_shared > // - (Fblocks, Enull, b, noiseModel_); + const size_t M = ZDim * m; + Matrix E0(M, M - 3); + computeJacobiansSVD(F, E0, b, cameras, point); + std::cout << M << std::endl; + SharedIsotropic n = noiseModel::Isotropic::Sigma(M, noiseModel_->sigma()); + return boost::make_shared >(F, E0, b, n); } private: diff --git a/gtsam/slam/SmartProjectionFactor.h b/gtsam/slam/SmartProjectionFactor.h index 2f4da3ce6..d932a1672 100644 --- a/gtsam/slam/SmartProjectionFactor.h +++ b/gtsam/slam/SmartProjectionFactor.h @@ -368,7 +368,12 @@ public: // ================================================================== Matrix F, E; Vector b; - double f = computeJacobians(F, E, b, cameras); + double f; + { + std::vector Fblocks; + f = computeJacobiansWithTriangulatedPoint(Fblocks, E, b, cameras); + Base::FillDiagonalF(Fblocks,F); // expensive ! + } // Schur complement trick // Frank says: should be possible to do this more efficiently? @@ -486,21 +491,12 @@ public: return nonDegenerate; } - /// Version that takes values, and creates the point - bool computeJacobians(std::vector& Fblocks, - Matrix& E, Vector& b, const Values& values) const { - Cameras cameras; - bool nonDegenerate = computeCamerasAndTriangulate(values, cameras); - if (nonDegenerate) - computeJacobians(Fblocks, E, b, cameras); - return nonDegenerate; - } - /// Compute F, E only (called below in both vanilla and SVD versions) /// Assumes the point has been computed /// Note E can be 2m*3 or 2m*2, in case point is degenerate - double computeJacobians(std::vector& Fblocks, - Matrix& E, Vector& b, const Cameras& cameras) const { + double computeJacobiansWithTriangulatedPoint( + std::vector& Fblocks, Matrix& E, Vector& b, + const Cameras& cameras) const { if (this->degenerate_) { std::cout << "manage degeneracy " << manageDegeneracy_ << std::endl; @@ -515,9 +511,9 @@ public: } // TODO replace all this by Call to CameraSet - int numKeys = this->keys_.size(); - E = zeros(2 * numKeys, 2); - b = zero(2 * numKeys); + 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++) { if (i == 0) { // first pose @@ -541,35 +537,27 @@ public: } // end else } + /// Version that takes values, and creates the point + bool triangulateAndComputeJacobians(std::vector& Fblocks, + Matrix& E, Vector& b, const Values& values) const { + Cameras cameras; + bool nonDegenerate = computeCamerasAndTriangulate(values, cameras); + if (nonDegenerate) + computeJacobiansWithTriangulatedPoint(Fblocks, E, b, cameras); + return nonDegenerate; + } + /// takes values - bool computeJacobiansSVD(std::vector& Fblocks, - Matrix& Enull, Vector& b, const Values& values) const { + bool triangulateAndComputeJacobiansSVD( + std::vector& Fblocks, Matrix& Enull, + Vector& b, const Values& values) const { typename Base::Cameras cameras; double good = computeCamerasAndTriangulate(values, cameras); if (good) - computeJacobiansSVD(Fblocks, Enull, b, cameras); + Base::computeJacobiansSVD(Fblocks, Enull, b, cameras, point_); return true; } - /// SVD version - double computeJacobiansSVD(std::vector& Fblocks, - Matrix& Enull, Vector& b, const Cameras& cameras) const { - return Base::computeJacobiansSVD(Fblocks, Enull, b, cameras, point_); - } - - /// Returns Matrix, TODO: maybe should not exist -> not sparse ! - // TODO should there be a lambda? - double computeJacobiansSVD(Matrix& F, Matrix& Enull, Vector& b, - const Cameras& cameras) const { - return Base::computeJacobiansSVD(F, Enull, b, cameras, point_); - } - - /// Returns Matrix, TODO: maybe should not exist -> not sparse ! - double computeJacobians(Matrix& F, Matrix& E, Vector& b, - const Cameras& cameras) const { - return Base::computeJacobians(F, E, b, cameras, point_); - } - /// Calculate vector of re-projection errors, before applying noise model Vector reprojectionErrorAfterTriangulation(const Values& values) const { Cameras cameras; @@ -652,6 +640,7 @@ public: inline bool isPointBehindCamera() const { return cheiralityException_; } + /** return cheirality verbosity */ inline bool verboseCheirality() const { return verboseCheirality_; diff --git a/gtsam/slam/tests/testSmartProjectionCameraFactor.cpp b/gtsam/slam/tests/testSmartProjectionCameraFactor.cpp index f82073e81..4ff53664d 100644 --- a/gtsam/slam/tests/testSmartProjectionCameraFactor.cpp +++ b/gtsam/slam/tests/testSmartProjectionCameraFactor.cpp @@ -754,17 +754,17 @@ TEST( SmartProjectionCameraFactor, computeImplicitJacobian ) { cameras.push_back(level_camera_right); factor1->error(values); // this is important to have a triangulation of the point - Point3 expectedPoint; + Point3 point; if (factor1->point()) - expectedPoint = *(factor1->point()); - factor1->computeJacobians(expectedF, expectedE, expectedb, cameras); + point = *(factor1->point()); + factor1->computeJacobians(expectedF, expectedE, expectedb, cameras, point); NonlinearFactorGraph generalGraph; SFMFactor sfm1(level_uv, unit2, c1, l1); SFMFactor sfm2(level_uv_right, unit2, c2, l1); generalGraph.push_back(sfm1); generalGraph.push_back(sfm2); - values.insert(l1, expectedPoint); // note: we get rid of possible errors in the triangulation + values.insert(l1, point); // note: we get rid of possible errors in the triangulation Matrix actualFullHessian = generalGraph.linearize(values)->hessian().first; Matrix actualVinv = (actualFullHessian.block(18, 18, 3, 3)).inverse(); diff --git a/gtsam/slam/tests/testSmartProjectionPoseFactor.cpp b/gtsam/slam/tests/testSmartProjectionPoseFactor.cpp index 41c0fac0a..78b8b5240 100644 --- a/gtsam/slam/tests/testSmartProjectionPoseFactor.cpp +++ b/gtsam/slam/tests/testSmartProjectionPoseFactor.cpp @@ -153,7 +153,7 @@ TEST_UNSAFE( SmartProjectionPoseFactor, noiseless ) { // Calculate using computeJacobians Vector b; vector Fblocks; - double actualError3 = factor.computeJacobians(Fblocks, E, b, cameras); + double actualError3 = factor.computeJacobians(Fblocks, E, b, cameras, *point); EXPECT(assert_equal(expectedE, E, 1e-7)); EXPECT_DOUBLES_EQUAL(expectedError, actualError3, 1e-8); } @@ -364,19 +364,19 @@ TEST( SmartProjectionPoseFactor, smartFactorWithSensorBodyTransform ) { // Calculate expected derivative for point (easiest to check) SmartFactor::Cameras cameras = smartFactor1->cameras(values); boost::function f = // - boost::bind(&SmartFactor::whitenedErrors, *smartFactor1, cameras, _1); + boost::bind(&SmartFactor::reprojectionError, *smartFactor1, cameras, _1); boost::optional point = smartFactor1->point(); CHECK(point); - // Note ! After refactor the noiseModel is only in the factors, not these matrices - Matrix expectedE = sigma * numericalDerivative11(f, *point); + // 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 whitenedError + // Calculate using reprojectionError Matrix E; SmartFactor::Cameras::FBlocks F; Vector actualErrors = smartFactor1->reprojectionError(cameras, *point, F, E); @@ -472,7 +472,7 @@ TEST( SmartProjectionPoseFactor, Factors ) { E(2, 2) = 10; E(3, 1) = 100; const vector > Fblocks = list_of > // - (make_pair(x1, 10 * F1))(make_pair(x2, 10 * F2)); + (make_pair(x1, F1))(make_pair(x2, F2)); Matrix3 P = (E.transpose() * E).inverse(); Vector4 b; b.setZero(); @@ -483,10 +483,11 @@ TEST( SmartProjectionPoseFactor, Factors ) { boost::shared_ptr > actual = smartFactor1->createRegularImplicitSchurFactor(cameras, 0.0); CHECK(actual); - CHECK(assert_equal(expected, *actual)); + EXPECT(assert_equal(expected, *actual)); // createJacobianQFactor - JacobianFactorQ<6, 2> expectedQ(Fblocks, E, P, b); + SharedIsotropic n = noiseModel::Isotropic::Sigma(4, sigma); + JacobianFactorQ<6, 2> expectedQ(Fblocks, E, P, b, n); boost::shared_ptr > actualQ = smartFactor1->createJacobianQFactor(cameras, 0.0); @@ -890,8 +891,10 @@ TEST( SmartProjectionPoseFactor, CheckHessian) { using namespace vanillaPose; // Two slightly different cameras - Pose3 pose2 = level_pose * Pose3(Rot3::RzRyRx(-0.05, 0.0, -0.05), Point3(0, 0, 0)); - Pose3 pose3 = level_pose * Pose3(Rot3::RzRyRx(-0.05, 0.0, -0.05), Point3(0, 0, 0)); + Pose3 pose2 = level_pose + * Pose3(Rot3::RzRyRx(-0.05, 0.0, -0.05), Point3(0, 0, 0)); + Pose3 pose3 = level_pose + * Pose3(Rot3::RzRyRx(-0.05, 0.0, -0.05), Point3(0, 0, 0)); Camera cam2(pose2, sharedK); Camera cam3(pose3, sharedK); @@ -970,8 +973,10 @@ TEST( SmartProjectionPoseFactor, 3poses_2land_rotation_only_smart_projection_fac views.push_back(x3); // Two different cameras - Pose3 pose2 = level_pose * Pose3(Rot3::RzRyRx(-0.05, 0.0, -0.05), Point3(0, 0, 0)); - Pose3 pose3 = level_pose * Pose3(Rot3::RzRyRx(-0.05, 0.0, -0.05), Point3(0, 0, 0)); + Pose3 pose2 = level_pose + * Pose3(Rot3::RzRyRx(-0.05, 0.0, -0.05), Point3(0, 0, 0)); + Pose3 pose3 = level_pose + * Pose3(Rot3::RzRyRx(-0.05, 0.0, -0.05), Point3(0, 0, 0)); Camera cam2(pose2, sharedK2); Camera cam3(pose3, sharedK2); @@ -1050,8 +1055,10 @@ TEST( SmartProjectionPoseFactor, 3poses_rotation_only_smart_projection_factor ) views.push_back(x3); // Two different cameras - Pose3 pose2 = level_pose * Pose3(Rot3::RzRyRx(-0.05, 0.0, -0.05), Point3(0, 0, 0)); - Pose3 pose3 = level_pose * Pose3(Rot3::RzRyRx(-0.05, 0.0, -0.05), Point3(0, 0, 0)); + Pose3 pose2 = level_pose + * Pose3(Rot3::RzRyRx(-0.05, 0.0, -0.05), Point3(0, 0, 0)); + Pose3 pose3 = level_pose + * Pose3(Rot3::RzRyRx(-0.05, 0.0, -0.05), Point3(0, 0, 0)); Camera cam2(pose2, sharedK); Camera cam3(pose3, sharedK); @@ -1390,8 +1397,10 @@ TEST( SmartProjectionPoseFactor, Cal3BundlerRotationOnly ) { views.push_back(x3); // Two different cameras - Pose3 pose2 = level_pose * Pose3(Rot3::RzRyRx(-0.05, 0.0, -0.05), Point3(0, 0, 0)); - Pose3 pose3 = level_pose * Pose3(Rot3::RzRyRx(-0.05, 0.0, -0.05), Point3(0, 0, 0)); + Pose3 pose2 = level_pose + * Pose3(Rot3::RzRyRx(-0.05, 0.0, -0.05), Point3(0, 0, 0)); + Pose3 pose3 = level_pose + * Pose3(Rot3::RzRyRx(-0.05, 0.0, -0.05), Point3(0, 0, 0)); Camera cam2(pose2, sharedBundlerK); Camera cam3(pose3, sharedBundlerK); diff --git a/gtsam_unstable/slam/SmartStereoProjectionFactor.h b/gtsam_unstable/slam/SmartStereoProjectionFactor.h index 337bf2f68..5c38ccca8 100644 --- a/gtsam_unstable/slam/SmartStereoProjectionFactor.h +++ b/gtsam_unstable/slam/SmartStereoProjectionFactor.h @@ -582,41 +582,16 @@ public: } // end else } -// /// Version that computes PointCov, with optional lambda parameter -// double computeJacobians(std::vector& Fblocks, -// Matrix& E, Matrix& PointCov, Vector& b, const Cameras& cameras, -// const double lambda = 0.0) const { -// -// double f = computeJacobians(Fblocks, E, b, cameras); -// -// // Point covariance inv(E'*E) -// PointCov.noalias() = (E.transpose() * E + lambda * eye(E.cols())).inverse(); -// -// return f; -// } -// -// /// takes values -// bool computeJacobiansSVD(std::vector& Fblocks, -// Matrix& Enull, Vector& b, const Values& values) const { -// typename Base::Cameras cameras; -// double good = computeCamerasAndTriangulate(values, cameras); -// if (good) -// computeJacobiansSVD(Fblocks, Enull, b, cameras); -// return true; -// } -// -// /// SVD version -// double computeJacobiansSVD(std::vector& Fblocks, -// Matrix& Enull, Vector& b, const Cameras& cameras) const { -// return Base::computeJacobiansSVD(Fblocks, Enull, b, cameras, point_); -// } -// -// /// Returns Matrix, TODO: maybe should not exist -> not sparse ! -// // TODO should there be a lambda? -// double computeJacobiansSVD(Matrix& F, Matrix& Enull, Vector& b, -// const Cameras& cameras) const { -// return Base::computeJacobiansSVD(F, Enull, b, cameras, point_); -// } + /// takes values + bool triangulateAndComputeJacobiansSVD( + std::vector& Fblocks, Matrix& Enull, + Vector& b, const Values& values) const { + typename Base::Cameras cameras; + double good = computeCamerasAndTriangulate(values, cameras); + if (good) + return Base::computeJacobiansSVD(Fblocks, Enull, b, cameras, point_); + return true; + } /// Returns Matrix, TODO: maybe should not exist -> not sparse ! double computeJacobians(Matrix& F, Matrix& E, Vector& b,