diff --git a/gtsam/geometry/CalibratedCamera.h b/gtsam/geometry/CalibratedCamera.h index feb004b71..1f791935d 100644 --- a/gtsam/geometry/CalibratedCamera.h +++ b/gtsam/geometry/CalibratedCamera.h @@ -32,9 +32,17 @@ namespace gtsam { class GTSAM_EXPORT CheiralityException: public ThreadsafeException< CheiralityException> { public: - CheiralityException() : - ThreadsafeException("Cheirality Exception") { - } + CheiralityException() + : CheiralityException(std::numeric_limits::max()) {} + + CheiralityException(Key j) + : ThreadsafeException("CheiralityException"), + j_(j) {} + + Key nearbyVariable() const {return j_;} + +private: + Key j_; }; /** diff --git a/gtsam/geometry/OrientedPlane3.cpp b/gtsam/geometry/OrientedPlane3.cpp index 4af0257ec..5cfa80779 100644 --- a/gtsam/geometry/OrientedPlane3.cpp +++ b/gtsam/geometry/OrientedPlane3.cpp @@ -83,8 +83,14 @@ Vector3 OrientedPlane3::errorVector(const OrientedPlane3& other, OptionalJacobia } /* ************************************************************************* */ -OrientedPlane3 OrientedPlane3::retract(const Vector3& v) const { - return OrientedPlane3(n_.retract(Vector2(v(0), v(1))), d_ + v(2)); +OrientedPlane3 OrientedPlane3::retract(const Vector3& v, + OptionalJacobian<3,3> H) const { + Matrix22 H_n; + Unit3 n_retract (n_.retract(Vector2(v(0), v(1)), H? &H_n : nullptr)); + if (H) { + *H << H_n, Vector2::Zero(), 0, 0, 1; + } + return OrientedPlane3(n_retract, d_ + v(2)); } /* ************************************************************************* */ diff --git a/gtsam/geometry/OrientedPlane3.h b/gtsam/geometry/OrientedPlane3.h index e425a4b81..596118385 100644 --- a/gtsam/geometry/OrientedPlane3.h +++ b/gtsam/geometry/OrientedPlane3.h @@ -134,7 +134,7 @@ public: } /// The retract function - OrientedPlane3 retract(const Vector3& v) const; + OrientedPlane3 retract(const Vector3& v, OptionalJacobian<3,3> H = boost::none) const; /// The local coordinates function Vector3 localCoordinates(const OrientedPlane3& s) const; diff --git a/gtsam/geometry/StereoCamera.h b/gtsam/geometry/StereoCamera.h index fa5d4ffde..3b5bdaefc 100644 --- a/gtsam/geometry/StereoCamera.h +++ b/gtsam/geometry/StereoCamera.h @@ -25,9 +25,19 @@ namespace gtsam { class GTSAM_EXPORT StereoCheiralityException: public std::runtime_error { public: - StereoCheiralityException() : - std::runtime_error("Stereo Cheirality Exception") { + StereoCheiralityException() + : StereoCheiralityException(std::numeric_limits::max()) {} + + StereoCheiralityException(Key j) + : std::runtime_error("Stereo Cheirality Exception"), + j_(j) {} + + Key nearbyVariable() const { + return j_; } + +private: + Key j_; }; /** diff --git a/gtsam/geometry/Unit3.cpp b/gtsam/geometry/Unit3.cpp index 08f14c829..f661f819d 100755 --- a/gtsam/geometry/Unit3.cpp +++ b/gtsam/geometry/Unit3.cpp @@ -250,19 +250,33 @@ double Unit3::distance(const Unit3& q, OptionalJacobian<1, 2> H) const { } /* ************************************************************************* */ -Unit3 Unit3::retract(const Vector2& v) const { +Unit3 Unit3::retract(const Vector2& v, OptionalJacobian<2,2> H) const { // Compute the 3D xi_hat vector const Vector3 xi_hat = basis() * v; const double theta = xi_hat.norm(); + const double c = std::cos(theta); - // Treat case of very small v differently + // Treat case of very small v differently. + Matrix23 H_from_point; if (theta < std::numeric_limits::epsilon()) { - return Unit3(Vector3(std::cos(theta) * p_ + xi_hat)); + const Unit3 exp_p_xi_hat = Unit3::FromPoint3(c * p_ + xi_hat, + H? &H_from_point : nullptr); + if (H) { // Jacobian + *H = H_from_point * + (-p_ * xi_hat.transpose() + Matrix33::Identity()) * basis(); + } + return exp_p_xi_hat; } - const Vector3 exp_p_xi_hat = - std::cos(theta) * p_ + xi_hat * (sin(theta) / theta); - return Unit3(exp_p_xi_hat); + const double st = std::sin(theta) / theta; + const Unit3 exp_p_xi_hat = Unit3::FromPoint3(c * p_ + xi_hat * st, + H? &H_from_point : nullptr); + if (H) { // Jacobian + *H = H_from_point * + (p_ * -st * xi_hat.transpose() + st * Matrix33::Identity() + + xi_hat * ((c - st) / std::pow(theta, 2)) * xi_hat.transpose()) * basis(); + } + return exp_p_xi_hat; } /* ************************************************************************* */ diff --git a/gtsam/geometry/Unit3.h b/gtsam/geometry/Unit3.h index 493c7d00a..f182df285 100644 --- a/gtsam/geometry/Unit3.h +++ b/gtsam/geometry/Unit3.h @@ -188,7 +188,7 @@ public: }; /// The retract function - Unit3 retract(const Vector2& v) const; + Unit3 retract(const Vector2& v, OptionalJacobian<2,2> H = boost::none) const; /// The local coordinates function Vector2 localCoordinates(const Unit3& s) const; diff --git a/gtsam/geometry/tests/testOrientedPlane3.cpp b/gtsam/geometry/tests/testOrientedPlane3.cpp index a1531e07c..54cf84ea8 100644 --- a/gtsam/geometry/tests/testOrientedPlane3.cpp +++ b/gtsam/geometry/tests/testOrientedPlane3.cpp @@ -161,6 +161,26 @@ TEST (OrientedPlane3, error2) { EXPECT(assert_equal(expectedH2, actualH2, 1e-9)); } +//******************************************************************************* +TEST (OrientedPlane3, jacobian_retract) { + OrientedPlane3 plane(-1, 0.1, 0.2, 5); + Matrix33 H_actual; + boost::function f = + boost::bind(&OrientedPlane3::retract, plane, _1, boost::none); + { + Vector3 v (-0.1, 0.2, 0.3); + plane.retract(v, H_actual); + Matrix H_expected_numerical = numericalDerivative11(f, v); + EXPECT(assert_equal(H_expected_numerical, H_actual, 1e-9)); + } + { + Vector3 v (0, 0, 0); + plane.retract(v, H_actual); + Matrix H_expected_numerical = numericalDerivative11(f, v); + EXPECT(assert_equal(H_expected_numerical, H_actual, 1e-9)); + } +} + /* ************************************************************************* */ int main() { srand(time(NULL)); diff --git a/gtsam/geometry/tests/testUnit3.cpp b/gtsam/geometry/tests/testUnit3.cpp index 542aca038..9d53a30a6 100644 --- a/gtsam/geometry/tests/testUnit3.cpp +++ b/gtsam/geometry/tests/testUnit3.cpp @@ -371,6 +371,26 @@ TEST(Unit3, retract) { } } +//******************************************************************************* +TEST (Unit3, jacobian_retract) { + Matrix22 H; + Unit3 p; + boost::function f = + boost::bind(&Unit3::retract, p, _1, boost::none); + { + Vector2 v (-0.2, 0.1); + p.retract(v, H); + Matrix H_expected_numerical = numericalDerivative11(f, v); + EXPECT(assert_equal(H_expected_numerical, H, 1e-9)); + } + { + Vector2 v (0, 0); + p.retract(v, H); + Matrix H_expected_numerical = numericalDerivative11(f, v); + EXPECT(assert_equal(H_expected_numerical, H, 1e-9)); + } +} + //******************************************************************************* TEST(Unit3, retract_expmap) { Unit3 p; diff --git a/gtsam/linear/tests/testGaussianBayesTree.cpp b/gtsam/linear/tests/testGaussianBayesTree.cpp index 2e8b60ac5..ca3ebf91c 100644 --- a/gtsam/linear/tests/testGaussianBayesTree.cpp +++ b/gtsam/linear/tests/testGaussianBayesTree.cpp @@ -297,6 +297,29 @@ TEST(GaussianBayesTree, ComputeSteepestDescentPointBT) { EXPECT(newError < origError); } +/* ************************************************************************* */ +TEST(GaussianBayesTree, determinant_and_smallestEigenvalue) { + + // create small factor graph + GaussianFactorGraph fg; + Key x1 = 2, x2 = 0, l1 = 1; + SharedDiagonal unit2 = noiseModel::Unit::Create(2); + fg += JacobianFactor(x1, 10 * I_2x2, -1.0 * Vector::Ones(2), unit2); + fg += JacobianFactor(x2, 10 * I_2x2, x1, -10 * I_2x2, Vector2(2.0, -1.0), unit2); + fg += JacobianFactor(l1, 5 * I_2x2, x1, -5 * I_2x2, Vector2(0.0, 1.0), unit2); + fg += JacobianFactor(x2, -5 * I_2x2, l1, 5 * I_2x2, Vector2(-1.0, 1.5), unit2); + + // create corresponding Bayes tree: + boost::shared_ptr bt = fg.eliminateMultifrontal(); + Matrix H = fg.hessian().first; + + // test determinant + // NOTE: the hessian of the factor graph is H = R'R where R is the matrix encoded by the bayes tree, + // for this reason we have to take the sqrt + double expectedDeterminant = sqrt(H.determinant()); // determinant computed from full matrix + double actualDeterminant = bt->determinant(); + EXPECT_DOUBLES_EQUAL(expectedDeterminant,actualDeterminant,expectedDeterminant*1e-6);// relative tolerance +} /* ************************************************************************* */ int main() { TestResult tr; return TestRegistry::runAllTests(tr);} diff --git a/gtsam/navigation/PreintegrationParams.h b/gtsam/navigation/PreintegrationParams.h index 3a2fb467a..0fb54a358 100644 --- a/gtsam/navigation/PreintegrationParams.h +++ b/gtsam/navigation/PreintegrationParams.h @@ -56,6 +56,7 @@ struct GTSAM_EXPORT PreintegrationParams: PreintegratedRotationParams { const Matrix3& getAccelerometerCovariance() const { return accelerometerCovariance; } const Matrix3& getIntegrationCovariance() const { return integrationCovariance; } + const Vector3& getGravity() const { return n_gravity; } bool getUse2ndOrderCoriolis() const { return use2ndOrderCoriolis; } protected: diff --git a/gtsam/slam/ProjectionFactor.h b/gtsam/slam/ProjectionFactor.h index 8332acabc..4a8a6b138 100644 --- a/gtsam/slam/ProjectionFactor.h +++ b/gtsam/slam/ProjectionFactor.h @@ -152,7 +152,7 @@ namespace gtsam { std::cout << e.what() << ": Landmark "<< DefaultKeyFormatter(this->key2()) << " moved behind camera " << DefaultKeyFormatter(this->key1()) << std::endl; if (throwCheirality_) - throw e; + throw CheiralityException(this->key2()); } return Vector2::Constant(2.0 * K_->fx()); } diff --git a/gtsam/slam/StereoFactor.h b/gtsam/slam/StereoFactor.h index 59fc372cb..8828f5de7 100644 --- a/gtsam/slam/StereoFactor.h +++ b/gtsam/slam/StereoFactor.h @@ -144,7 +144,7 @@ public: std::cout << e.what() << ": Landmark "<< DefaultKeyFormatter(this->key2()) << " moved behind camera " << DefaultKeyFormatter(this->key1()) << std::endl; if (throwCheirality_) - throw e; + throw StereoCheiralityException(this->key2()); } return Vector3::Constant(2.0 * K_->fx()); } diff --git a/gtsam_unstable/nonlinear/BatchFixedLagSmoother.cpp b/gtsam_unstable/nonlinear/BatchFixedLagSmoother.cpp index 56de7feab..4077464a9 100644 --- a/gtsam_unstable/nonlinear/BatchFixedLagSmoother.cpp +++ b/gtsam_unstable/nonlinear/BatchFixedLagSmoother.cpp @@ -52,7 +52,7 @@ Matrix BatchFixedLagSmoother::marginalCovariance(Key key) const { /* ************************************************************************* */ FixedLagSmoother::Result BatchFixedLagSmoother::update( const NonlinearFactorGraph& newFactors, const Values& newTheta, - const KeyTimestampMap& timestamps) { + const KeyTimestampMap& timestamps, const FastVector& factorsToRemove) { // Update all of the internal variables with the new information gttic(augment_system); @@ -69,6 +69,12 @@ FixedLagSmoother::Result BatchFixedLagSmoother::update( insertFactors(newFactors); gttoc(augment_system); + // remove factors in factorToRemove + for(const size_t i : factorsToRemove){ + if(factors_[i]) + factors_[i].reset(); + } + // Update the Timestamps associated with the factor keys updateKeyTimestampMap(timestamps); diff --git a/gtsam_unstable/nonlinear/BatchFixedLagSmoother.h b/gtsam_unstable/nonlinear/BatchFixedLagSmoother.h index 27fe94451..2bb99b7e6 100644 --- a/gtsam_unstable/nonlinear/BatchFixedLagSmoother.h +++ b/gtsam_unstable/nonlinear/BatchFixedLagSmoother.h @@ -47,8 +47,10 @@ public: virtual bool equals(const FixedLagSmoother& rhs, double tol = 1e-9) const; /** Add new factors, updating the solution and relinearizing as needed. */ - Result update(const NonlinearFactorGraph& newFactors = NonlinearFactorGraph(), const Values& newTheta = Values(), - const KeyTimestampMap& timestamps = KeyTimestampMap()); + Result update(const NonlinearFactorGraph& newFactors = NonlinearFactorGraph(), + const Values& newTheta = Values(), + const KeyTimestampMap& timestamps = KeyTimestampMap(), + const FastVector& factorsToRemove = FastVector()); /** Compute an estimate from the incomplete linear delta computed during the last update. * This delta is incomplete because it was not updated below wildfire_threshold. If only diff --git a/gtsam_unstable/nonlinear/FixedLagSmoother.cpp b/gtsam_unstable/nonlinear/FixedLagSmoother.cpp index 62ee07a16..9c04f0eec 100644 --- a/gtsam_unstable/nonlinear/FixedLagSmoother.cpp +++ b/gtsam_unstable/nonlinear/FixedLagSmoother.cpp @@ -21,6 +21,15 @@ namespace gtsam { +/* ************************************************************************* */ +void FixedLagSmoother::Result::print() const { + std::cout << "Nr iterations: " << iterations << '\n' + << "Nr intermediateSteps: " << intermediateSteps << '\n' + << "Nr nonlinear variables: " << nonlinearVariables << '\n' + << "Nr linear variables: " << linearVariables << '\n' + << "error: " << error << std::endl; +} + /* ************************************************************************* */ void FixedLagSmoother::print(const std::string& s, const KeyFormatter& keyFormatter) const { std::cout << s; diff --git a/gtsam_unstable/nonlinear/FixedLagSmoother.h b/gtsam_unstable/nonlinear/FixedLagSmoother.h index 4868a7cf1..1d8321eb3 100644 --- a/gtsam_unstable/nonlinear/FixedLagSmoother.h +++ b/gtsam_unstable/nonlinear/FixedLagSmoother.h @@ -59,9 +59,9 @@ public: size_t getNonlinearVariables() const { return nonlinearVariables; } size_t getLinearVariables() const { return linearVariables; } double getError() const { return error; } + void print() const; }; - /** default constructor */ FixedLagSmoother(double smootherLag = 0.0) : smootherLag_(smootherLag) { } @@ -90,8 +90,10 @@ public: } /** Add new factors, updating the solution and relinearizing as needed. */ - virtual Result update(const NonlinearFactorGraph& newFactors = NonlinearFactorGraph(), const Values& newTheta = Values(), - const KeyTimestampMap& timestamps = KeyTimestampMap()) = 0; + virtual Result update(const NonlinearFactorGraph& newFactors = NonlinearFactorGraph(), + const Values& newTheta = Values(), + const KeyTimestampMap& timestamps = KeyTimestampMap(), + const FastVector& factorsToRemove = FastVector()) = 0; /** Compute an estimate from the incomplete linear delta computed during the last update. * This delta is incomplete because it was not updated below wildfire_threshold. If only diff --git a/gtsam_unstable/nonlinear/IncrementalFixedLagSmoother.cpp b/gtsam_unstable/nonlinear/IncrementalFixedLagSmoother.cpp index d3c34ff5e..689a3697c 100644 --- a/gtsam_unstable/nonlinear/IncrementalFixedLagSmoother.cpp +++ b/gtsam_unstable/nonlinear/IncrementalFixedLagSmoother.cpp @@ -65,7 +65,7 @@ bool IncrementalFixedLagSmoother::equals(const FixedLagSmoother& rhs, /* ************************************************************************* */ FixedLagSmoother::Result IncrementalFixedLagSmoother::update( const NonlinearFactorGraph& newFactors, const Values& newTheta, - const KeyTimestampMap& timestamps) { + const KeyTimestampMap& timestamps, const FastVector& factorsToRemove) { const bool debug = ISDEBUG("IncrementalFixedLagSmoother update"); @@ -125,8 +125,8 @@ FixedLagSmoother::Result IncrementalFixedLagSmoother::update( KeyList additionalMarkedKeys(additionalKeys.begin(), additionalKeys.end()); // Update iSAM2 - ISAM2Result isamResult = isam_.update(newFactors, newTheta, - FactorIndices(), constrainedKeys, boost::none, additionalMarkedKeys); + isamResult_ = isam_.update(newFactors, newTheta, + factorsToRemove, constrainedKeys, boost::none, additionalMarkedKeys); if (debug) { PrintSymbolicTree(isam_, diff --git a/gtsam_unstable/nonlinear/IncrementalFixedLagSmoother.h b/gtsam_unstable/nonlinear/IncrementalFixedLagSmoother.h index 77d052a21..0014a0747 100644 --- a/gtsam_unstable/nonlinear/IncrementalFixedLagSmoother.h +++ b/gtsam_unstable/nonlinear/IncrementalFixedLagSmoother.h @@ -59,10 +59,12 @@ public: * @param newFactors new factors on old and/or new variables * @param newTheta new values for new variables only * @param timestamps an (optional) map from keys to real time stamps + * @param factorsToRemove an (optional) list of factors to remove. */ Result update(const NonlinearFactorGraph& newFactors = NonlinearFactorGraph(), - const Values& newTheta = Values(), // - const KeyTimestampMap& timestamps = KeyTimestampMap()); + const Values& newTheta = Values(), // + const KeyTimestampMap& timestamps = KeyTimestampMap(), + const FastVector& factorsToRemove = FactorIndices()); /** Compute an estimate from the incomplete linear delta computed during the last update. * This delta is incomplete because it was not updated below wildfire_threshold. If only @@ -108,11 +110,17 @@ public: return isam_.marginalCovariance(key); } + /// Get results of latest isam2 update + const ISAM2Result& getISAM2Result() const{ return isamResult_; } + protected: /** An iSAM2 object used to perform inference. The smoother lag is controlled * by what factors are removed each iteration */ ISAM2 isam_; + /** Store results of latest isam2 update */ + ISAM2Result isamResult_; + /** Erase any keys associated with timestamps before the provided time */ void eraseKeysBefore(double timestamp); diff --git a/gtsam_unstable/nonlinear/tests/testBatchFixedLagSmoother.cpp b/gtsam_unstable/nonlinear/tests/testBatchFixedLagSmoother.cpp index cd36da398..20a7beff5 100644 --- a/gtsam_unstable/nonlinear/tests/testBatchFixedLagSmoother.cpp +++ b/gtsam_unstable/nonlinear/tests/testBatchFixedLagSmoother.cpp @@ -53,11 +53,11 @@ TEST( BatchFixedLagSmoother, Example ) // the BatchFixedLagSmoother should be identical (even with the linearized approximations at // the end of the smoothing lag) -// SETDEBUG("BatchFixedLagSmoother update", true); -// SETDEBUG("BatchFixedLagSmoother reorder", true); -// SETDEBUG("BatchFixedLagSmoother optimize", true); -// SETDEBUG("BatchFixedLagSmoother marginalize", true); -// SETDEBUG("BatchFixedLagSmoother calculateMarginalFactors", true); + // SETDEBUG("BatchFixedLagSmoother update", true); + // SETDEBUG("BatchFixedLagSmoother reorder", true); + // SETDEBUG("BatchFixedLagSmoother optimize", true); + // SETDEBUG("BatchFixedLagSmoother marginalize", true); + // SETDEBUG("BatchFixedLagSmoother calculateMarginalFactors", true); // Set up parameters SharedDiagonal odometerNoise = noiseModel::Diagonal::Sigmas(Vector2(0.1, 0.1)); @@ -177,6 +177,65 @@ TEST( BatchFixedLagSmoother, Example ) ++i; } + // add/remove an extra factor + { + Key key1 = Key(i-1); + Key key2 = Key(i); + + NonlinearFactorGraph newFactors; + Values newValues; + Timestamps newTimestamps; + + // add 2 odometry factors + newFactors.push_back(BetweenFactor(key1, key2, Point2(1.0, 0.0), odometerNoise)); + newFactors.push_back(BetweenFactor(key1, key2, Point2(1.0, 0.0), odometerNoise)); + newValues.insert(key2, Point2(double(i)+0.1, -0.1)); + newTimestamps[key2] = double(i); + + fullgraph.push_back(newFactors); + fullinit.insert(newValues); + + // Update the smoother + smoother.update(newFactors, newValues, newTimestamps); + + // Check + CHECK(check_smoother(fullgraph, fullinit, smoother, key2)); + +// NonlinearFactorGraph smootherGraph = smoother.getFactors(); +// for(size_t i=0; iprint(); +// } +// } + + // now remove one of the two and try again + // empty values and new factors for fake update in which we only remove factors + NonlinearFactorGraph emptyNewFactors; + Values emptyNewValues; + Timestamps emptyNewTimestamps; + + size_t factorIndex = 6; // any index that does not break connectivity of the graph + FastVector factorToRemove; + factorToRemove.push_back(factorIndex); + + const NonlinearFactorGraph smootherFactorsBeforeRemove = smoother.getFactors(); + + // remove factor + smoother.update(emptyNewFactors, emptyNewValues, emptyNewTimestamps,factorToRemove); + + // check that the factors in the smoother are right + NonlinearFactorGraph actual = smoother.getFactors(); + for(size_t i=0; i< smootherFactorsBeforeRemove.size(); i++){ + // check that the factors that were not removed are there + if(smootherFactorsBeforeRemove[i] && i != factorIndex){ + EXPECT(smootherFactorsBeforeRemove[i]->equals(*actual[i])); + } + else{ // while the factors that were not there or were removed are no longer there + EXPECT(!actual[i]); + } + } + } } /* ************************************************************************* */ diff --git a/gtsam_unstable/nonlinear/tests/testIncrementalFixedLagSmoother.cpp b/gtsam_unstable/nonlinear/tests/testIncrementalFixedLagSmoother.cpp index 65df6261c..ab6c1298a 100644 --- a/gtsam_unstable/nonlinear/tests/testIncrementalFixedLagSmoother.cpp +++ b/gtsam_unstable/nonlinear/tests/testIncrementalFixedLagSmoother.cpp @@ -71,8 +71,6 @@ TEST( IncrementalFixedLagSmoother, Example ) Values fullinit; NonlinearFactorGraph fullgraph; - - // i keeps track of the time step size_t i = 0; @@ -177,6 +175,63 @@ TEST( IncrementalFixedLagSmoother, Example ) ++i; } + // add/remove an extra factor + { + Key key1 = MakeKey(i-1); + Key key2 = MakeKey(i); + + NonlinearFactorGraph newFactors; + Values newValues; + Timestamps newTimestamps; + + // add 2 odometry factors + newFactors.push_back(BetweenFactor(key1, key2, Point2(1.0, 0.0), odometerNoise)); + newFactors.push_back(BetweenFactor(key1, key2, Point2(1.0, 0.0), odometerNoise)); + newValues.insert(key2, Point2(double(i)+0.1, -0.1)); + newTimestamps[key2] = double(i); + + fullgraph.push_back(newFactors); + fullinit.insert(newValues); + + // Update the smoother + smoother.update(newFactors, newValues, newTimestamps); + + // Check + CHECK(check_smoother(fullgraph, fullinit, smoother, key2)); + + // now remove one of the two and try again + // empty values and new factors for fake update in which we only remove factors + NonlinearFactorGraph emptyNewFactors; + Values emptyNewValues; + Timestamps emptyNewTimestamps; + + size_t factorIndex = 25; // any index that does not break connectivity of the graph + FastVector factorToRemove; + factorToRemove.push_back(factorIndex); + + const NonlinearFactorGraph smootherFactorsBeforeRemove = smoother.getFactors(); + + // remove factor + smoother.update(emptyNewFactors, emptyNewValues, emptyNewTimestamps,factorToRemove); + + // Note: the following test (checking that the number of factor is reduced by 1) + // fails since we are not reusing slots, hence also when removing a factor we do not change + // the size of the factor graph + // size_t nrFactorsAfterRemoval = smoother.getFactors().size(); + // DOUBLES_EQUAL(nrFactorsBeforeRemoval-1, nrFactorsAfterRemoval, 1e-5); + + // check that the factors in the smoother are right + NonlinearFactorGraph actual = smoother.getFactors(); + for(size_t i=0; i< smootherFactorsBeforeRemove.size(); i++){ + // check that the factors that were not removed are there + if(smootherFactorsBeforeRemove[i] && i != factorIndex){ + EXPECT(smootherFactorsBeforeRemove[i]->equals(*actual[i])); + } + else{ // while the factors that were not there or were removed are no longer there + EXPECT(!actual[i]); + } + } + } } /* ************************************************************************* */