12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

fixed unit test!

release/4.3a0
lcarlone 2016-08-04 23:38:39 -04:00
parent 4e8968e3f5
commit b59a58278b
1 changed files with 21 additions and 22 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

43
gtsam_unstable/nonlinear/tests/testIncrementalFixedLagSmoother.cpp
View File

@ -200,38 +200,37 @@ TEST( IncrementalFixedLagSmoother, Example )
CHECK(check_smoother(fullgraph, fullinit, smoother, key2));
// now remove one of the two and try again
size_t factorIndex = fullgraph.size()-2; // any index that does not break connectivity of the graph
FastVector<size_t> factorToRemove;
factorToRemove.push_back(factorIndex);
// empty values and new factors for fake update in which we only remove factors
NonlinearFactorGraph emptyNewFactors;
Values emptyNewValues;
Timestamps emptyNewTimestamps;
//smoother.getFactors().print();
size_t factorIndex = 25; // any index that does not break connectivity of the graph
FastVector<size_t> factorToRemove;
factorToRemove.push_back(factorIndex);
const NonlinearFactorGraph& allFactors = smoother.getFactors();
size_t nrFactorsBeforeRemoval = allFactors.size();
NonlinearFactorGraph expected;
for(size_t i=0; i< allFactors.size(); i++){
// if(i != factorIndex)
// expected.push_back( allFactors[i] );
// else
std::cout << "ind: " << i << std::endl;
allFactors[i]->print();
}
const NonlinearFactorGraph smootherFactorsBeforeRemove = smoother.getFactors();
// remove factor
smoother.update(emptyNewFactors, emptyNewValues, emptyNewTimestamps, factorToRemove);
size_t nrFactorsAfterRemoval = smoother.getFactors().size();
smoother.update(emptyNewFactors, emptyNewValues, emptyNewTimestamps,factorToRemove);
// check that the number of factors is right
DOUBLES_EQUAL(nrFactorsBeforeRemoval-1, nrFactorsAfterRemoval, 1e-5);
// 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();
//CHECK(assert_equal(expected,actual));
//smoother.getFactors().print();
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]);
}
}
}
}