iSAM2 factorization parameter to choose LDL or QR
Richard Roberts
parent
74a5a81745
commit
ba8fb0ba27
|
|
@ -163,7 +163,7 @@ void ISAM2::Impl::ExpmapMasked(Values& values, const Permuted<VectorValues>& del
|
|||
/* ************************************************************************* */
|
||||
ISAM2::Impl::PartialSolveResult
|
||||
ISAM2::Impl::PartialSolve(GaussianFactorGraph& factors,
|
||||
const FastSet<Index>& keys, const ReorderingMode& reorderingMode) {
|
||||
const FastSet<Index>& keys, const ReorderingMode& reorderingMode, bool useQR) {
|
||||
|
||||
const bool debug = ISDEBUG("ISAM2 recalculate");
|
||||
|
||||
|
|
@ -245,7 +245,10 @@ ISAM2::Impl::PartialSolve(GaussianFactorGraph& factors,
|
|||
// eliminate into a Bayes net
|
||||
tic(7,"eliminate");
|
||||
JunctionTree<GaussianFactorGraph, ISAM2::Clique> jt(factors, affectedFactorsIndex);
|
||||
result.bayesTree = jt.eliminate(EliminatePreferLDL);
|
||||
if(!useQR)
|
||||
result.bayesTree = jt.eliminate(EliminatePreferLDL);
|
||||
else
|
||||
result.bayesTree = jt.eliminate(EliminateQR);
|
||||
toc(7,"eliminate");
|
||||
|
||||
tic(8,"permute eliminated");
|
||||
|
|
|
|||
|
|
@ -118,11 +118,12 @@ struct ISAM2::Impl {
|
|||
* problem. This is permuted during use and so is cleared when this function
|
||||
* returns in order to invalidate it.
|
||||
* \param keys The set of indices used in \c factors.
|
||||
* \param useQR Whether to use QR (if true), or LDL (if false).
|
||||
* \return The eliminated BayesTree and the permutation to be applied to the
|
||||
* rest of the ISAM2 data.
|
||||
*/
|
||||
static PartialSolveResult PartialSolve(GaussianFactorGraph& factors, const FastSet<Index>& keys,
|
||||
const ReorderingMode& reorderingMode);
|
||||
const ReorderingMode& reorderingMode, bool useQR);
|
||||
|
||||
static size_t UpdateDelta(const boost::shared_ptr<ISAM2Clique>& root, std::vector<bool>& replacedKeys, Permuted<VectorValues>& delta, double wildfireThreshold);
|
||||
|
||||
|
|
|
|||
|
|
@ -123,12 +123,11 @@ ISAM2::relinearizeAffectedFactors(const FastList<Index>& affectedKeys) const {
|
|||
|
||||
/* ************************************************************************* */
|
||||
// find intermediate (linearized) factors from cache that are passed into the affected area
|
||||
FactorGraph<ISAM2::CacheFactor>
|
||||
ISAM2::getCachedBoundaryFactors(Cliques& orphans) {
|
||||
GaussianFactorGraph ISAM2::getCachedBoundaryFactors(Cliques& orphans) {
|
||||
|
||||
static const bool debug = false;
|
||||
|
||||
FactorGraph<CacheFactor> cachedBoundary;
|
||||
GaussianFactorGraph cachedBoundary;
|
||||
|
||||
BOOST_FOREACH(sharedClique orphan, orphans) {
|
||||
// find the last variable that was eliminated
|
||||
|
|
@ -141,7 +140,7 @@ ISAM2::getCachedBoundaryFactors(Cliques& orphans) {
|
|||
// assert(key == lastKey);
|
||||
#endif
|
||||
// retrieve the cached factor and add to boundary
|
||||
cachedBoundary.push_back(boost::dynamic_pointer_cast<CacheFactor>(orphan->cachedFactor()));
|
||||
cachedBoundary.push_back(orphan->cachedFactor());
|
||||
if(debug) { cout << "Cached factor for variable " << key; orphan->cachedFactor()->print(""); }
|
||||
}
|
||||
|
||||
|
|
@ -266,7 +265,12 @@ boost::shared_ptr<FastSet<Index> > ISAM2::recalculate(
|
|||
|
||||
tic(5,"eliminate");
|
||||
JunctionTree<GaussianFactorGraph, Base::Clique> jt(factors, variableIndex_);
|
||||
sharedClique newRoot = jt.eliminate(EliminatePreferLDL);
|
||||
sharedClique newRoot;
|
||||
if(params_.factorization == ISAM2Params::LDL)
|
||||
newRoot = jt.eliminate(EliminatePreferLDL);
|
||||
else if(params_.factorization == ISAM2Params::QR)
|
||||
newRoot = jt.eliminate(EliminateQR);
|
||||
else assert(false);
|
||||
if(debug) newRoot->print("Eliminated: ");
|
||||
toc(5,"eliminate");
|
||||
|
||||
|
|
@ -314,12 +318,12 @@ boost::shared_ptr<FastSet<Index> > ISAM2::recalculate(
|
|||
|
||||
tic(2,"cached");
|
||||
// add the cached intermediate results from the boundary of the orphans ...
|
||||
FactorGraph<CacheFactor> cachedBoundary = getCachedBoundaryFactors(orphans);
|
||||
GaussianFactorGraph cachedBoundary = getCachedBoundaryFactors(orphans);
|
||||
if(debug) cachedBoundary.print("Boundary factors: ");
|
||||
factors.reserve(factors.size() + cachedBoundary.size());
|
||||
// Copy so that we can later permute factors
|
||||
BOOST_FOREACH(const CacheFactor::shared_ptr& cached, cachedBoundary) {
|
||||
factors.push_back(GaussianFactor::shared_ptr(new CacheFactor(*cached)));
|
||||
BOOST_FOREACH(const GaussianFactor::shared_ptr& cached, cachedBoundary) {
|
||||
factors.push_back(cached->clone());
|
||||
}
|
||||
toc(2,"cached");
|
||||
|
||||
|
|
@ -348,7 +352,7 @@ boost::shared_ptr<FastSet<Index> > ISAM2::recalculate(
|
|||
BOOST_FOREACH(Index var, newKeys) { reorderingMode.constrainedKeys->insert(make_pair(var, 1)); }
|
||||
}
|
||||
Impl::PartialSolveResult partialSolveResult =
|
||||
Impl::PartialSolve(factors, *affectedKeysSet, reorderingMode);
|
||||
Impl::PartialSolve(factors, *affectedKeysSet, reorderingMode, (params_.factorization == ISAM2Params::QR));
|
||||
toc(2,"PartialSolve");
|
||||
|
||||
// We now need to permute everything according this partial reordering: the
|
||||
|
|
|
|||
|
|
@ -107,6 +107,17 @@ struct ISAM2Params {
|
|||
|
||||
bool evaluateNonlinearError; ///< Whether to evaluate the nonlinear error before and after the update, to return in ISAM2Result from update()
|
||||
|
||||
enum Factorization { LDL, QR };
|
||||
/** Specifies whether to use QR or LDL numerical factorization (default: LDL).
|
||||
* LDL is faster but potentially numerically unstable for poorly-conditioned problems, which can occur when
|
||||
* uncertainty is very low in some variables (or dimensions of variables) and very high in others. QR is
|
||||
* slower but more numerically stable in poorly-conditioned problems. We suggest using the default of LDL
|
||||
* unless gtsam sometimes throws NegativeMatrixException when your problem's Hessian is actually positive
|
||||
* definite. For positive definite problems, numerical error accumulation can cause the problem to become
|
||||
* numerically negative or indefinite as solving proceeds, especially when using LDL.
|
||||
*/
|
||||
Factorization factorization;
|
||||
|
||||
KeyFormatter keyFormatter; ///< A KeyFormatter for when keys are printed during debugging (default: DefaultKeyFormatter)
|
||||
|
||||
/** Specify parameters as constructor arguments */
|
||||
|
|
@ -116,10 +127,12 @@ struct ISAM2Params {
|
|||
int _relinearizeSkip = 10, ///< see ISAM2Params::relinearizeSkip
|
||||
bool _enableRelinearization = true, ///< see ISAM2Params::enableRelinearization
|
||||
bool _evaluateNonlinearError = false, ///< see ISAM2Params::evaluateNonlinearError
|
||||
Factorization _factorization = ISAM2Params::LDL, ///< see ISAM2Params::factorization
|
||||
const KeyFormatter& _keyFormatter = DefaultKeyFormatter ///< see ISAM2::Params::keyFormatter
|
||||
) : optimizationParams(_optimizationParams), relinearizeThreshold(_relinearizeThreshold),
|
||||
relinearizeSkip(_relinearizeSkip), enableRelinearization(_enableRelinearization),
|
||||
evaluateNonlinearError(_evaluateNonlinearError), keyFormatter(_keyFormatter) {}
|
||||
evaluateNonlinearError(_evaluateNonlinearError), factorization(_factorization),
|
||||
keyFormatter(_keyFormatter) {}
|
||||
};
|
||||
|
||||
/**
|
||||
|
|
@ -340,8 +353,6 @@ private:
|
|||
std::vector<bool> lastRelinVariables_;
|
||||
#endif
|
||||
|
||||
typedef HessianFactor CacheFactor;
|
||||
|
||||
public:
|
||||
|
||||
typedef BayesTree<GaussianConditional,ISAM2Clique> Base; ///< The BayesTree base class
|
||||
|
|
@ -460,7 +471,7 @@ private:
|
|||
|
||||
FastList<size_t> getAffectedFactors(const FastList<Index>& keys) const;
|
||||
FactorGraph<GaussianFactor>::shared_ptr relinearizeAffectedFactors(const FastList<Index>& affectedKeys) const;
|
||||
FactorGraph<CacheFactor> getCachedBoundaryFactors(Cliques& orphans);
|
||||
GaussianFactorGraph getCachedBoundaryFactors(Cliques& orphans);
|
||||
|
||||
boost::shared_ptr<FastSet<Index> > recalculate(const FastSet<Index>& markedKeys,
|
||||
const FastVector<Index>& newKeys, const FactorGraph<GaussianFactor>::shared_ptr newFactors,
|
||||
|
|
|
|||
|
|
@ -509,6 +509,272 @@ TEST(ISAM2, slamlike_solution_dogleg)
|
|||
EXPECT(assert_equal(expectedGradient, actualGradient));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST(ISAM2, slamlike_solution_gaussnewton_qr)
|
||||
{
|
||||
|
||||
// SETDEBUG("ISAM2 update", true);
|
||||
// SETDEBUG("ISAM2 update verbose", true);
|
||||
// SETDEBUG("ISAM2 recalculate", true);
|
||||
|
||||
// Pose and landmark key types from planarSLAM
|
||||
using planarSLAM::PoseKey;
|
||||
using planarSLAM::PointKey;
|
||||
|
||||
// Set up parameters
|
||||
SharedDiagonal odoNoise = sharedSigmas(Vector_(3, 0.1, 0.1, M_PI/100.0));
|
||||
SharedDiagonal brNoise = sharedSigmas(Vector_(2, M_PI/100.0, 0.1));
|
||||
|
||||
// These variables will be reused and accumulate factors and values
|
||||
ISAM2 isam(ISAM2Params(ISAM2GaussNewtonParams(0.001), 0.0, 0, false, false, ISAM2Params::QR));
|
||||
Values fullinit;
|
||||
planarSLAM::Graph fullgraph;
|
||||
|
||||
// i keeps track of the time step
|
||||
size_t i = 0;
|
||||
|
||||
// Add a prior at time 0 and update isam
|
||||
{
|
||||
planarSLAM::Graph newfactors;
|
||||
newfactors.addPrior(0, Pose2(0.0, 0.0, 0.0), odoNoise);
|
||||
fullgraph.push_back(newfactors);
|
||||
|
||||
Values init;
|
||||
init.insert(PoseKey(0), Pose2(0.01, 0.01, 0.01));
|
||||
fullinit.insert(PoseKey(0), Pose2(0.01, 0.01, 0.01));
|
||||
|
||||
isam.update(newfactors, init);
|
||||
}
|
||||
|
||||
CHECK(isam_check(fullgraph, fullinit, isam));
|
||||
|
||||
// Add odometry from time 0 to time 5
|
||||
for( ; i<5; ++i) {
|
||||
planarSLAM::Graph newfactors;
|
||||
newfactors.addOdometry(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise);
|
||||
fullgraph.push_back(newfactors);
|
||||
|
||||
Values init;
|
||||
init.insert(PoseKey(i+1), Pose2(double(i+1)+0.1, -0.1, 0.01));
|
||||
fullinit.insert(PoseKey(i+1), Pose2(double(i+1)+0.1, -0.1, 0.01));
|
||||
|
||||
isam.update(newfactors, init);
|
||||
}
|
||||
|
||||
// Add odometry from time 5 to 6 and landmark measurement at time 5
|
||||
{
|
||||
planarSLAM::Graph newfactors;
|
||||
newfactors.addOdometry(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise);
|
||||
newfactors.addBearingRange(i, 0, Rot2::fromAngle(M_PI/4.0), 5.0, brNoise);
|
||||
newfactors.addBearingRange(i, 1, Rot2::fromAngle(-M_PI/4.0), 5.0, brNoise);
|
||||
fullgraph.push_back(newfactors);
|
||||
|
||||
Values init;
|
||||
init.insert(PoseKey(i+1), Pose2(1.01, 0.01, 0.01));
|
||||
init.insert(PointKey(0), Point2(5.0/sqrt(2.0), 5.0/sqrt(2.0)));
|
||||
init.insert(PointKey(1), Point2(5.0/sqrt(2.0), -5.0/sqrt(2.0)));
|
||||
fullinit.insert(PoseKey(i+1), Pose2(1.01, 0.01, 0.01));
|
||||
fullinit.insert(PointKey(0), Point2(5.0/sqrt(2.0), 5.0/sqrt(2.0)));
|
||||
fullinit.insert(PointKey(1), Point2(5.0/sqrt(2.0), -5.0/sqrt(2.0)));
|
||||
|
||||
isam.update(newfactors, init);
|
||||
++ i;
|
||||
}
|
||||
|
||||
// Add odometry from time 6 to time 10
|
||||
for( ; i<10; ++i) {
|
||||
planarSLAM::Graph newfactors;
|
||||
newfactors.addOdometry(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise);
|
||||
fullgraph.push_back(newfactors);
|
||||
|
||||
Values init;
|
||||
init.insert(PoseKey(i+1), Pose2(double(i+1)+0.1, -0.1, 0.01));
|
||||
fullinit.insert(PoseKey(i+1), Pose2(double(i+1)+0.1, -0.1, 0.01));
|
||||
|
||||
isam.update(newfactors, init);
|
||||
}
|
||||
|
||||
// Add odometry from time 10 to 11 and landmark measurement at time 10
|
||||
{
|
||||
planarSLAM::Graph newfactors;
|
||||
newfactors.addOdometry(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise);
|
||||
newfactors.addBearingRange(i, 0, Rot2::fromAngle(M_PI/4.0 + M_PI/16.0), 4.5, brNoise);
|
||||
newfactors.addBearingRange(i, 1, Rot2::fromAngle(-M_PI/4.0 + M_PI/16.0), 4.5, brNoise);
|
||||
fullgraph.push_back(newfactors);
|
||||
|
||||
Values init;
|
||||
init.insert(PoseKey(i+1), Pose2(6.9, 0.1, 0.01));
|
||||
fullinit.insert(PoseKey(i+1), Pose2(6.9, 0.1, 0.01));
|
||||
|
||||
isam.update(newfactors, init);
|
||||
++ i;
|
||||
}
|
||||
|
||||
// Compare solutions
|
||||
CHECK(isam_check(fullgraph, fullinit, isam));
|
||||
|
||||
// Check gradient at each node
|
||||
typedef ISAM2::sharedClique sharedClique;
|
||||
BOOST_FOREACH(const sharedClique& clique, isam.nodes()) {
|
||||
// Compute expected gradient
|
||||
FactorGraph<JacobianFactor> jfg;
|
||||
jfg.push_back(JacobianFactor::shared_ptr(new JacobianFactor(*clique->conditional())));
|
||||
VectorValues expectedGradient(*allocateVectorValues(isam));
|
||||
gradientAtZero(jfg, expectedGradient);
|
||||
// Compare with actual gradients
|
||||
int variablePosition = 0;
|
||||
for(GaussianConditional::const_iterator jit = clique->conditional()->begin(); jit != clique->conditional()->end(); ++jit) {
|
||||
const int dim = clique->conditional()->dim(jit);
|
||||
Vector actual = clique->gradientContribution().segment(variablePosition, dim);
|
||||
EXPECT(assert_equal(expectedGradient[*jit], actual));
|
||||
variablePosition += dim;
|
||||
}
|
||||
LONGS_EQUAL(clique->gradientContribution().rows(), variablePosition);
|
||||
}
|
||||
|
||||
// Check gradient
|
||||
VectorValues expectedGradient(*allocateVectorValues(isam));
|
||||
gradientAtZero(FactorGraph<JacobianFactor>(isam), expectedGradient);
|
||||
VectorValues expectedGradient2(gradient(FactorGraph<JacobianFactor>(isam), VectorValues::Zero(expectedGradient)));
|
||||
VectorValues actualGradient(*allocateVectorValues(isam));
|
||||
gradientAtZero(isam, actualGradient);
|
||||
EXPECT(assert_equal(expectedGradient2, expectedGradient));
|
||||
EXPECT(assert_equal(expectedGradient, actualGradient));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST(ISAM2, slamlike_solution_dogleg_qr)
|
||||
{
|
||||
|
||||
// SETDEBUG("ISAM2 update", true);
|
||||
// SETDEBUG("ISAM2 update verbose", true);
|
||||
// SETDEBUG("ISAM2 recalculate", true);
|
||||
|
||||
// Pose and landmark key types from planarSLAM
|
||||
using planarSLAM::PoseKey;
|
||||
using planarSLAM::PointKey;
|
||||
|
||||
// Set up parameters
|
||||
SharedDiagonal odoNoise = sharedSigmas(Vector_(3, 0.1, 0.1, M_PI/100.0));
|
||||
SharedDiagonal brNoise = sharedSigmas(Vector_(2, M_PI/100.0, 0.1));
|
||||
|
||||
// These variables will be reused and accumulate factors and values
|
||||
ISAM2 isam(ISAM2Params(ISAM2DoglegParams(1.0), 0.0, 0, false, false, ISAM2Params::QR));
|
||||
Values fullinit;
|
||||
planarSLAM::Graph fullgraph;
|
||||
|
||||
// i keeps track of the time step
|
||||
size_t i = 0;
|
||||
|
||||
// Add a prior at time 0 and update isam
|
||||
{
|
||||
planarSLAM::Graph newfactors;
|
||||
newfactors.addPrior(0, Pose2(0.0, 0.0, 0.0), odoNoise);
|
||||
fullgraph.push_back(newfactors);
|
||||
|
||||
Values init;
|
||||
init.insert(PoseKey(0), Pose2(0.01, 0.01, 0.01));
|
||||
fullinit.insert(PoseKey(0), Pose2(0.01, 0.01, 0.01));
|
||||
|
||||
isam.update(newfactors, init);
|
||||
}
|
||||
|
||||
CHECK(isam_check(fullgraph, fullinit, isam));
|
||||
|
||||
// Add odometry from time 0 to time 5
|
||||
for( ; i<5; ++i) {
|
||||
planarSLAM::Graph newfactors;
|
||||
newfactors.addOdometry(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise);
|
||||
fullgraph.push_back(newfactors);
|
||||
|
||||
Values init;
|
||||
init.insert(PoseKey(i+1), Pose2(double(i+1)+0.1, -0.1, 0.01));
|
||||
fullinit.insert(PoseKey(i+1), Pose2(double(i+1)+0.1, -0.1, 0.01));
|
||||
|
||||
isam.update(newfactors, init);
|
||||
}
|
||||
|
||||
// Add odometry from time 5 to 6 and landmark measurement at time 5
|
||||
{
|
||||
planarSLAM::Graph newfactors;
|
||||
newfactors.addOdometry(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise);
|
||||
newfactors.addBearingRange(i, 0, Rot2::fromAngle(M_PI/4.0), 5.0, brNoise);
|
||||
newfactors.addBearingRange(i, 1, Rot2::fromAngle(-M_PI/4.0), 5.0, brNoise);
|
||||
fullgraph.push_back(newfactors);
|
||||
|
||||
Values init;
|
||||
init.insert(PoseKey(i+1), Pose2(1.01, 0.01, 0.01));
|
||||
init.insert(PointKey(0), Point2(5.0/sqrt(2.0), 5.0/sqrt(2.0)));
|
||||
init.insert(PointKey(1), Point2(5.0/sqrt(2.0), -5.0/sqrt(2.0)));
|
||||
fullinit.insert(PoseKey(i+1), Pose2(1.01, 0.01, 0.01));
|
||||
fullinit.insert(PointKey(0), Point2(5.0/sqrt(2.0), 5.0/sqrt(2.0)));
|
||||
fullinit.insert(PointKey(1), Point2(5.0/sqrt(2.0), -5.0/sqrt(2.0)));
|
||||
|
||||
isam.update(newfactors, init);
|
||||
++ i;
|
||||
}
|
||||
|
||||
// Add odometry from time 6 to time 10
|
||||
for( ; i<10; ++i) {
|
||||
planarSLAM::Graph newfactors;
|
||||
newfactors.addOdometry(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise);
|
||||
fullgraph.push_back(newfactors);
|
||||
|
||||
Values init;
|
||||
init.insert(PoseKey(i+1), Pose2(double(i+1)+0.1, -0.1, 0.01));
|
||||
fullinit.insert(PoseKey(i+1), Pose2(double(i+1)+0.1, -0.1, 0.01));
|
||||
|
||||
isam.update(newfactors, init);
|
||||
}
|
||||
|
||||
// Add odometry from time 10 to 11 and landmark measurement at time 10
|
||||
{
|
||||
planarSLAM::Graph newfactors;
|
||||
newfactors.addOdometry(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise);
|
||||
newfactors.addBearingRange(i, 0, Rot2::fromAngle(M_PI/4.0 + M_PI/16.0), 4.5, brNoise);
|
||||
newfactors.addBearingRange(i, 1, Rot2::fromAngle(-M_PI/4.0 + M_PI/16.0), 4.5, brNoise);
|
||||
fullgraph.push_back(newfactors);
|
||||
|
||||
Values init;
|
||||
init.insert(PoseKey(i+1), Pose2(6.9, 0.1, 0.01));
|
||||
fullinit.insert(PoseKey(i+1), Pose2(6.9, 0.1, 0.01));
|
||||
|
||||
isam.update(newfactors, init);
|
||||
++ i;
|
||||
}
|
||||
|
||||
// Compare solutions
|
||||
CHECK(isam_check(fullgraph, fullinit, isam));
|
||||
|
||||
// Check gradient at each node
|
||||
typedef ISAM2::sharedClique sharedClique;
|
||||
BOOST_FOREACH(const sharedClique& clique, isam.nodes()) {
|
||||
// Compute expected gradient
|
||||
FactorGraph<JacobianFactor> jfg;
|
||||
jfg.push_back(JacobianFactor::shared_ptr(new JacobianFactor(*clique->conditional())));
|
||||
VectorValues expectedGradient(*allocateVectorValues(isam));
|
||||
gradientAtZero(jfg, expectedGradient);
|
||||
// Compare with actual gradients
|
||||
int variablePosition = 0;
|
||||
for(GaussianConditional::const_iterator jit = clique->conditional()->begin(); jit != clique->conditional()->end(); ++jit) {
|
||||
const int dim = clique->conditional()->dim(jit);
|
||||
Vector actual = clique->gradientContribution().segment(variablePosition, dim);
|
||||
EXPECT(assert_equal(expectedGradient[*jit], actual));
|
||||
variablePosition += dim;
|
||||
}
|
||||
LONGS_EQUAL(clique->gradientContribution().rows(), variablePosition);
|
||||
}
|
||||
|
||||
// Check gradient
|
||||
VectorValues expectedGradient(*allocateVectorValues(isam));
|
||||
gradientAtZero(FactorGraph<JacobianFactor>(isam), expectedGradient);
|
||||
VectorValues expectedGradient2(gradient(FactorGraph<JacobianFactor>(isam), VectorValues::Zero(expectedGradient)));
|
||||
VectorValues actualGradient(*allocateVectorValues(isam));
|
||||
gradientAtZero(isam, actualGradient);
|
||||
EXPECT(assert_equal(expectedGradient2, expectedGradient));
|
||||
EXPECT(assert_equal(expectedGradient, actualGradient));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST(ISAM2, clone) {
|
||||
|
||||
|
|
|
|||
Loading…
Reference in New Issue