Making all Bayes tests testing both dense and sparse

This commit is contained in:
matlabbe
2026-08-19 21:44:52 -07:00
parent 261bf9dfa2
commit fa02cc1a0f
6 changed files with 187 additions and 56 deletions
+13 -3
View File
@@ -148,6 +148,14 @@ public:
*/ */
float getVirtualPlacePrior() const; float getVirtualPlacePrior() const;
/**
* @brief Whether the prediction is being kept in its sparse form rather than as a matrix.
*
* False when @ref Parameters::kBayesSparsePrediction() is disabled, and while it is enabled
* over a prediction measured as too dense for the sparse form to be worth it.
*/
bool isPredictionSparse() const;
/** /**
* @brief Returns the loop-closure prediction model as a vector of values. * @brief Returns the loop-closure prediction model as a vector of values.
* @return Vector in the format `{Vp, Lc, l1, l2, l3, ...}`. * @return Vector in the format `{Vp, Lc, l1, l2, l3, ...}`.
@@ -167,11 +175,13 @@ public:
* transition probabilities according to @ref getPredictionLC(). When @p ids match the * transition probabilities according to @ref getPredictionLC(). When @p ids match the
* current posterior keys, the cached matrix may be returned without recomputation. * current posterior keys, the cached matrix may be returned without recomputation.
* *
* When the prediction is being kept sparse, the matrix is expanded from it rather than
* kept: it costs the memory that keeping the prediction sparse is saving, so ask for it to
* read, dump or compare the prediction, not on every iteration.
*
* @param memory Working memory instance (must not be null). * @param memory Working memory instance (must not be null).
* @param ids Ordered list of signature ids (often includes @ref Memory::kIdVirtual as first element). * @param ids Ordered list of signature ids (often includes @ref Memory::kIdVirtual as first element).
* @return Square CV_32FC1 matrix of size ids.size() × ids.size(), or an empty matrix when * @return Square CV_32FC1 matrix of size ids.size() × ids.size().
* @ref Parameters::kBayesSparsePrediction() is enabled and the prediction is being
* kept in its sparse form, in which case no matrix exists to return.
*/ */
cv::Mat generatePrediction(const Memory * memory, const std::vector<int> & ids); cv::Mat generatePrediction(const Memory * memory, const std::vector<int> & ids);
+9 -3
View File
@@ -114,6 +114,11 @@ float BayesFilter::getVirtualPlacePrior() const
return _model->virtualPlacePrior(); return _model->virtualPlacePrior();
} }
bool BayesFilter::isPredictionSparse() const
{
return !_sparse->empty();
}
void BayesFilter::reset() void BayesFilter::reset()
{ {
_posteriorIds.clear(); _posteriorIds.clear();
@@ -294,10 +299,11 @@ bool BayesFilter::computePosterior(const Memory * memory, const std::map<int, fl
cv::Mat BayesFilter::generatePrediction(const Memory * memory, const std::vector<int> & ids) cv::Mat BayesFilter::generatePrediction(const Memory * memory, const std::vector<int> & ids)
{ {
if(!_sparse->empty()) if(!_sparse->empty() && _sparse->ids() == ids)
{ {
// The prediction is being kept sparse, so there is no matrix to return. // Expanded from the sparse form, which holds the same prediction. The matrix costs
return cv::Mat(); // what keeping it sparse is saving, so it is built to be read and not kept.
return _sparse->toMatrix();
} }
if(!_dense->empty() && _dense->ids() == ids) if(!_dense->empty() && _dense->ids() == ids)
{ {
-9
View File
@@ -5820,15 +5820,6 @@ void Rtabmap::dumpPrediction() const
} }
} }
cv::Mat prediction = _bayesFilter->generatePrediction(_memory, uListToVector(signaturesToCompare)); cv::Mat prediction = _bayesFilter->generatePrediction(_memory, uListToVector(signaturesToCompare));
if(prediction.empty())
{
UERROR("The prediction matrix cannot be exported while %s is enabled: the "
"prediction is kept in a sparse form and the matrix is never built. Set "
"%s=false to export it.",
Parameters::kBayesSparsePrediction().c_str(),
Parameters::kBayesSparsePrediction().c_str());
return;
}
FILE* fout = 0; FILE* fout = 0;
std::string fileName = this->getWorkingDir() + "/DumpPrediction.txt"; std::string fileName = this->getWorkingDir() + "/DumpPrediction.txt";
+19
View File
@@ -42,6 +42,25 @@ void SparsePrediction::clear()
used_ = 0; used_ = 0;
} }
cv::Mat SparsePrediction::toMatrix() const
{
const int size = (int)columns_.size();
if(size == 0)
{
return cv::Mat();
}
cv::Mat matrix = cv::Mat::zeros(size, size, CV_32FC1);
for(int col=0; col<size; ++col)
{
const Column & slot = columns_[col];
for(size_t i=slot.offset; i<slot.offset+slot.size; ++i)
{
matrix.at<float>(values_[i].first, col) = values_[i].second;
}
}
return matrix;
}
unsigned long SparsePrediction::memoryUsed() const unsigned long SparsePrediction::memoryUsed() const
{ {
return values_.capacity() * sizeof(std::pair<int, float>) return values_.capacity() * sizeof(std::pair<int, float>)
+9
View File
@@ -30,6 +30,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include "bayes/PredictionModel.h" #include "bayes/PredictionModel.h"
#include <opencv2/core/core.hpp>
#include <utility> #include <utility>
#include <vector> #include <vector>
@@ -87,6 +88,14 @@ public:
/// prior = prediction x posterior. /// prior = prediction x posterior.
void multiply(const std::vector<float> & posterior, std::vector<float> & prior) const; void multiply(const std::vector<float> & posterior, std::vector<float> & prior) const;
/**
* @brief The same prediction as a matrix, for the one caller that wants to look at it.
*
* The matrix costs what keeping the prediction sparse is saving, so this builds one to be
* read, dumped or compared against DensePrediction, and does not keep it.
*/
cv::Mat toMatrix() const;
unsigned long memoryUsed() const; unsigned long memoryUsed() const;
private: private:
+137 -41
View File
@@ -261,6 +261,8 @@ protected:
ParametersMap params; ParametersMap params;
params.insert(ParametersPair(Parameters::kBayesPredictionLC(), kPredictionNewPlace10Stay50Neighbor25_15)); params.insert(ParametersPair(Parameters::kBayesPredictionLC(), kPredictionNewPlace10Stay50Neighbor25_15));
params.insert(ParametersPair(Parameters::kBayesFullPredictionUpdate(), fullPredictionUpdate ? "true" : "false")); params.insert(ParametersPair(Parameters::kBayesFullPredictionUpdate(), fullPredictionUpdate ? "true" : "false"));
// The matrix is what this hands back, so the prediction is not kept sparse.
params.insert(ParametersPair(Parameters::kBayesSparsePrediction(), "false"));
BayesFilter filter(params); BayesFilter filter(params);
std::vector<int> ids = getBayesIds(); std::vector<int> ids = getBayesIds();
@@ -286,6 +288,40 @@ protected:
// Constructor Tests // Constructor Tests
// Bayes/SparsePrediction changes how the prediction is held and multiplied, not what the
// posterior is, so every test below that reads the posterior runs in both forms. The ones
// that read the prediction matrix instead keep the dense form, which is the only one that
// has a matrix.
class BayesFilterModeTest : public ::testing::TestWithParam<bool>
{
protected:
ParametersMap modeParams(ParametersMap params = ParametersMap()) const
{
params[Parameters::kBayesSparsePrediction()] = GetParam() ? "true" : "false";
return params;
}
};
class BayesFilterMemoryModeFixture :
public BayesFilterMemoryFixture,
public ::testing::WithParamInterface<bool>
{
protected:
ParametersMap modeParams(ParametersMap params = ParametersMap()) const
{
params[Parameters::kBayesSparsePrediction()] = GetParam() ? "true" : "false";
return params;
}
};
static std::string predictionFormName(const ::testing::TestParamInfo<bool> & info)
{
return info.param ? "Sparse" : "Dense";
}
INSTANTIATE_TEST_SUITE_P(PredictionForm, BayesFilterModeTest, ::testing::Bool(), predictionFormName);
INSTANTIATE_TEST_SUITE_P(PredictionForm, BayesFilterMemoryModeFixture, ::testing::Bool(), predictionFormName);
TEST(BayesFilterTest, DefaultConstructor) TEST(BayesFilterTest, DefaultConstructor)
{ {
BayesFilter filter; BayesFilter filter;
@@ -376,10 +412,10 @@ TEST(BayesFilterTest, GetPredictionLCStrRoundTrip)
// reset Tests // reset Tests
TEST(BayesFilterTest, Reset) TEST_P(BayesFilterModeTest, Reset)
{ {
Memory memory; Memory memory;
ParametersMap params; ParametersMap params = modeParams();
params.insert(ParametersPair(Parameters::kBayesPredictionLC(), kPredictionNewPlace10Stay90)); params.insert(ParametersPair(Parameters::kBayesPredictionLC(), kPredictionNewPlace10Stay90));
BayesFilter filter(params); BayesFilter filter(params);
@@ -394,9 +430,9 @@ TEST(BayesFilterTest, Reset)
// getMemoryUsed Tests // getMemoryUsed Tests
TEST(BayesFilterTest, GetMemoryUsed) TEST_P(BayesFilterModeTest, GetMemoryUsed)
{ {
BayesFilter filter; BayesFilter filter(modeParams());
EXPECT_GE(filter.getMemoryUsed(), sizeof(BayesFilter)); EXPECT_GE(filter.getMemoryUsed(), sizeof(BayesFilter));
Memory memory; Memory memory;
@@ -430,9 +466,11 @@ TEST(BayesFilterTest, ComputePosteriorEmptyLikelihood)
// generatePrediction Tests (virtual place only, no graph) // generatePrediction Tests (virtual place only, no graph)
TEST(BayesFilterTest, GeneratePredictionVirtualPlaceOnly) // Nothing has been computed yet, so there is no sparse form to keep and the matrix is built
// in both cases.
TEST_P(BayesFilterModeTest, GeneratePredictionVirtualPlaceOnly)
{ {
ParametersMap params; ParametersMap params = modeParams();
params.insert(ParametersPair(Parameters::kBayesVirtualPlacePriorThr(), "0.9")); params.insert(ParametersPair(Parameters::kBayesVirtualPlacePriorThr(), "0.9"));
BayesFilter filter(params); BayesFilter filter(params);
@@ -468,9 +506,9 @@ TEST(BayesFilterTest, GeneratePredictionCachedWhenIdsUnchanged)
EXPECT_EQ(prediction1.data, prediction2.data); EXPECT_EQ(prediction1.data, prediction2.data);
} }
TEST(BayesFilterTest, ComputePosteriorVirtualPlaceOnly) TEST_P(BayesFilterModeTest, ComputePosteriorVirtualPlaceOnly)
{ {
ParametersMap params; ParametersMap params = modeParams();
params.insert(ParametersPair(Parameters::kBayesPredictionLC(), kPredictionNewPlace10Stay90)); params.insert(ParametersPair(Parameters::kBayesPredictionLC(), kPredictionNewPlace10Stay90));
BayesFilter filter(params); BayesFilter filter(params);
@@ -486,9 +524,9 @@ TEST(BayesFilterTest, ComputePosteriorVirtualPlaceOnly)
EXPECT_TRUE(approxEqual(posterior.begin()->second, 1.0f)); EXPECT_TRUE(approxEqual(posterior.begin()->second, 1.0f));
} }
TEST(BayesFilterTest, ComputePosteriorNormalizesPosterior) TEST_P(BayesFilterModeTest, ComputePosteriorNormalizesPosterior)
{ {
ParametersMap params; ParametersMap params = modeParams();
params.insert(ParametersPair(Parameters::kBayesPredictionLC(), kPredictionNewPlace10Stay90)); params.insert(ParametersPair(Parameters::kBayesPredictionLC(), kPredictionNewPlace10Stay90));
BayesFilter filter(params); BayesFilter filter(params);
@@ -503,9 +541,9 @@ TEST(BayesFilterTest, ComputePosteriorNormalizesPosterior)
EXPECT_TRUE(approxEqual(posterior.begin()->second, 1.0f)); EXPECT_TRUE(approxEqual(posterior.begin()->second, 1.0f));
} }
TEST(BayesFilterTest, ComputePosteriorUpdatesWithNewLikelihood) TEST_P(BayesFilterModeTest, ComputePosteriorUpdatesWithNewLikelihood)
{ {
ParametersMap params; ParametersMap params = modeParams();
params.insert(ParametersPair(Parameters::kBayesPredictionLC(), kPredictionNewPlace10Stay90)); params.insert(ParametersPair(Parameters::kBayesPredictionLC(), kPredictionNewPlace10Stay90));
BayesFilter filter(params); BayesFilter filter(params);
@@ -524,6 +562,10 @@ TEST(BayesFilterTest, ComputePosteriorUpdatesWithNewLikelihood)
// Integration tests with real Memory graph // Integration tests with real Memory graph
// The columns of the prediction, read from the matrix. A map this small is never kept sparse,
// a column of it reaching more than a quarter of the locations, so this is the dense form
// whatever Bayes/SparsePrediction says; GeneratePredictionExpandsTheSparseFormIntoTheSameMatrix
// carries these values over to the sparse form on a map large enough to keep it.
TEST_F(BayesFilterMemoryFixture, GeneratePredictionLinearChain) TEST_F(BayesFilterMemoryFixture, GeneratePredictionLinearChain)
{ {
addChain(5); addChain(5);
@@ -534,6 +576,7 @@ TEST_F(BayesFilterMemoryFixture, GeneratePredictionLinearChain)
ParametersMap params; ParametersMap params;
params.insert(ParametersPair(Parameters::kBayesPredictionLC(), kPredictionNewPlace10Stay50Neighbor25_15)); params.insert(ParametersPair(Parameters::kBayesPredictionLC(), kPredictionNewPlace10Stay50Neighbor25_15));
params.insert(ParametersPair(Parameters::kBayesVirtualPlacePriorThr(), "0.9")); params.insert(ParametersPair(Parameters::kBayesVirtualPlacePriorThr(), "0.9"));
params.insert(ParametersPair(Parameters::kBayesSparsePrediction(), "false"));
BayesFilter filter(params); BayesFilter filter(params);
const std::vector<int> ids = getBayesIds(); const std::vector<int> ids = getBayesIds();
@@ -592,11 +635,14 @@ TEST_F(BayesFilterMemoryFixture, GeneratePredictionLinearChain)
(float)predictionLC[1]}); (float)predictionLC[1]});
} }
TEST_F(BayesFilterMemoryFixture, GeneratePredictionNormalizesWhenSumBelowOne) // A model whose values sum to less than 1 has normalize() spread the difference over every
// zero of a column, so there is nothing sparse to keep and the matrix is the form used with
// Bayes/SparsePrediction enabled as well as disabled. The columns below are its values.
TEST_P(BayesFilterMemoryModeFixture, GeneratePredictionNormalizesWhenSumBelowOne)
{ {
addChain(4); addChain(4);
ParametersMap params; ParametersMap params = modeParams();
params.insert(ParametersPair(Parameters::kBayesPredictionLC(), kPredictionSumBelowOne)); params.insert(ParametersPair(Parameters::kBayesPredictionLC(), kPredictionSumBelowOne));
BayesFilter filter(params); BayesFilter filter(params);
@@ -604,6 +650,9 @@ TEST_F(BayesFilterMemoryFixture, GeneratePredictionNormalizesWhenSumBelowOne)
ASSERT_EQ(ids.size(), 4u); // virtual + 3 visited WM nodes (latest in STM) ASSERT_EQ(ids.size(), 4u); // virtual + 3 visited WM nodes (latest in STM)
ASSERT_EQ(ids[0], Memory::kIdVirtual); ASSERT_EQ(ids[0], Memory::kIdVirtual);
ASSERT_TRUE(filter.computePosterior(memory_, uniformLikelihood(ids)));
ASSERT_FALSE(filter.isPredictionSparse());
const std::vector<double> & predictionLC = filter.getPredictionLC(); const std::vector<double> & predictionLC = filter.getPredictionLC();
ASSERT_EQ(predictionLC.size(), 2u); ASSERT_EQ(predictionLC.size(), 2u);
@@ -647,29 +696,54 @@ TEST_F(BayesFilterMemoryFixture, GeneratePredictionNormalizesWhenSumBelowOne)
(float)predictionLC[1] * scaleRatio}); (float)predictionLC[1] * scaleRatio});
} }
TEST_F(BayesFilterMemoryFixture, GeneratePredictionCachedWithGraph) // The matrix is kept between iterations and handed over as it is while the ids do not change.
// The sparse form is expanded into a matrix per call instead: it is the caller asking to read
// the prediction that pays for it, not every iteration.
TEST_P(BayesFilterMemoryModeFixture, GeneratePredictionCachedWithGraph)
{ {
addChain(4); // Large enough for the sparse form to be worth keeping, so that both forms are exercised.
addChain(40);
ParametersMap params; ParametersMap params = modeParams();
params.insert(ParametersPair(Parameters::kBayesPredictionLC(), kPredictionNewPlace10Stay70Neighbor20)); params.insert(ParametersPair(Parameters::kBayesPredictionLC(), kPredictionNewPlace10Stay70Neighbor20));
BayesFilter filter(params); BayesFilter filter(params);
const std::vector<int> ids = getBayesIds(); const std::vector<int> ids = getBayesIds();
std::map<int, float> likelihood = uniformLikelihood(ids); std::map<int, float> likelihood = uniformLikelihood(ids);
filter.computePosterior(memory_, likelihood); ASSERT_TRUE(filter.computePosterior(memory_, likelihood));
ASSERT_EQ(filter.isPredictionSparse(), GetParam());
cv::Mat prediction1 = filter.generatePrediction(memory_, ids); cv::Mat prediction1 = filter.generatePrediction(memory_, ids);
cv::Mat prediction2 = filter.generatePrediction(memory_, ids); cv::Mat prediction2 = filter.generatePrediction(memory_, ids);
ASSERT_FALSE(prediction1.empty()); ASSERT_EQ(prediction1.rows, (int)ids.size());
EXPECT_EQ(prediction1.data, prediction2.data); ASSERT_EQ(prediction2.rows, (int)ids.size());
for(int col = 0; col < prediction1.cols; ++col)
{
for(int row = 0; row < prediction1.rows; ++row)
{
ASSERT_FLOAT_EQ(predictionAt(prediction1, row, col),
predictionAt(prediction2, row, col))
<< "row=" << row << " col=" << col;
}
}
// The matrix is the one held, handed over as it is. The sparse form has none to hand over
// and is expanded into one per call: same values, another buffer. Handing over the same one
// would mean keeping the expansion, which is the allocation the sparse form is avoiding.
if(filter.isPredictionSparse())
{
EXPECT_NE(prediction1.data, prediction2.data);
}
else
{
EXPECT_EQ(prediction1.data, prediction2.data);
}
} }
TEST_F(BayesFilterMemoryFixture, ComputePosteriorNormalizedWithGraph) TEST_P(BayesFilterMemoryModeFixture, ComputePosteriorNormalizedWithGraph)
{ {
addChain(16); addChain(16);
ParametersMap params; ParametersMap params = modeParams();
params.insert(ParametersPair(Parameters::kBayesPredictionLC(), kPredictionNewPlace10Stay50Neighbor25_15)); params.insert(ParametersPair(Parameters::kBayesPredictionLC(), kPredictionNewPlace10Stay50Neighbor25_15));
BayesFilter filter(params); BayesFilter filter(params);
@@ -709,11 +783,11 @@ TEST_F(BayesFilterMemoryFixture, ComputePosteriorNormalizedWithGraph)
EXPECT_NEAR(sum, 1.0f, 1e-4f); EXPECT_NEAR(sum, 1.0f, 1e-4f);
} }
TEST_F(BayesFilterMemoryFixture, ComputePosteriorFavorsHighLikelihood) TEST_P(BayesFilterMemoryModeFixture, ComputePosteriorFavorsHighLikelihood)
{ {
addChain(16); addChain(16);
ParametersMap params; ParametersMap params = modeParams();
params.insert(ParametersPair(Parameters::kBayesPredictionLC(), kPredictionNewPlace10Stay70Neighbor20)); params.insert(ParametersPair(Parameters::kBayesPredictionLC(), kPredictionNewPlace10Stay70Neighbor20));
BayesFilter filter(params); BayesFilter filter(params);
@@ -772,11 +846,11 @@ TEST_F(BayesFilterMemoryFixture, ComputePosteriorFavorsHighLikelihood)
} }
} }
TEST_F(BayesFilterMemoryFixture, ComputePosteriorSequentialIterations) TEST_P(BayesFilterMemoryModeFixture, ComputePosteriorSequentialIterations)
{ {
initMemory(5); initMemory(5);
ParametersMap params; ParametersMap params = modeParams();
params.insert(ParametersPair(Parameters::kBayesPredictionLC(), kPredictionNewPlace10Stay50Neighbor25_15)); params.insert(ParametersPair(Parameters::kBayesPredictionLC(), kPredictionNewPlace10Stay50Neighbor25_15));
params.insert(ParametersPair(Parameters::kBayesFullPredictionUpdate(), "true")); params.insert(ParametersPair(Parameters::kBayesFullPredictionUpdate(), "true"));
BayesFilter filter(params); BayesFilter filter(params);
@@ -826,13 +900,13 @@ TEST_F(BayesFilterMemoryFixture, ComputePosteriorSequentialIterations)
} }
} }
TEST_F(BayesFilterMemoryFixture, ComputePosteriorSequentialIterationsWithLoopClosures) TEST_P(BayesFilterMemoryModeFixture, ComputePosteriorSequentialIterationsWithLoopClosures)
{ {
// STM size 5: current signature stays in STM, so loop closures target older WM nodes // STM size 5: current signature stays in STM, so loop closures target older WM nodes
// (not the odometry neighbor link to the node added in the previous iteration). // (not the odometry neighbor link to the node added in the previous iteration).
initMemory(5); initMemory(5);
ParametersMap params; ParametersMap params = modeParams();
params.insert(ParametersPair(Parameters::kBayesPredictionLC(), kPredictionNewPlace10Stay50Neighbor25_15)); params.insert(ParametersPair(Parameters::kBayesPredictionLC(), kPredictionNewPlace10Stay50Neighbor25_15));
params.insert(ParametersPair(Parameters::kBayesFullPredictionUpdate(), "true")); params.insert(ParametersPair(Parameters::kBayesFullPredictionUpdate(), "true"));
BayesFilter filter(params); BayesFilter filter(params);
@@ -915,6 +989,8 @@ TEST_F(BayesFilterMemoryFixture, CompareFullPredictionUpdateModes)
ParametersMap paramsIncremental; ParametersMap paramsIncremental;
paramsIncremental.insert(ParametersPair(Parameters::kBayesPredictionLC(), kPredictionNewPlace10Stay50Neighbor25_15)); paramsIncremental.insert(ParametersPair(Parameters::kBayesPredictionLC(), kPredictionNewPlace10Stay50Neighbor25_15));
paramsIncremental.insert(ParametersPair(Parameters::kBayesFullPredictionUpdate(), "false")); paramsIncremental.insert(ParametersPair(Parameters::kBayesFullPredictionUpdate(), "false"));
// Both modes are compared column by column, so the matrix has to be there to compare.
paramsIncremental.insert(ParametersPair(Parameters::kBayesSparsePrediction(), "false"));
ParametersMap paramsFull = paramsIncremental; ParametersMap paramsFull = paramsIncremental;
paramsFull[Parameters::kBayesFullPredictionUpdate()] = "true"; paramsFull[Parameters::kBayesFullPredictionUpdate()] = "true";
@@ -989,6 +1065,7 @@ TEST_F(BayesFilterMemoryFixture, FullPredictionUpdateRegeneratesMatrix)
ParametersMap params; ParametersMap params;
params.insert(ParametersPair(Parameters::kBayesPredictionLC(), kPredictionNewPlace10Stay70Neighbor20)); params.insert(ParametersPair(Parameters::kBayesPredictionLC(), kPredictionNewPlace10Stay70Neighbor20));
params.insert(ParametersPair(Parameters::kBayesSparsePrediction(), "false"));
auto expectMatrixGrowsOnNewNode = [&](bool fullPredictionUpdate) { auto expectMatrixGrowsOnNewNode = [&](bool fullPredictionUpdate) {
params[Parameters::kBayesFullPredictionUpdate()] = fullPredictionUpdate ? "true" : "false"; params[Parameters::kBayesFullPredictionUpdate()] = fullPredictionUpdate ? "true" : "false";
@@ -1332,8 +1409,8 @@ TEST_F(BayesFilterMemoryFixture, PredictionCrossesFromSparseBackToTheMatrix)
<< "iter=" << iter << " id=" << ids[i]; << "iter=" << iter << " id=" << ids[i];
} }
} }
// Grown into the sparse form: there is no matrix left to hand over. // Grown into the sparse form, so the matrix of the first iterations is gone.
ASSERT_TRUE(filterSparse.generatePrediction(memory_, ids).empty()); ASSERT_TRUE(filterSparse.isPredictionSparse());
// Back to the matrix, which was last built 40 iterations and as many locations ago. It // Back to the matrix, which was last built 40 iterations and as many locations ago. It
// has to be built again rather than updated against locations it was never built for. // has to be built again rather than updated against locations it was never built for.
@@ -1351,6 +1428,7 @@ TEST_F(BayesFilterMemoryFixture, PredictionCrossesFromSparseBackToTheMatrix)
ASSERT_TRUE(filterDense.computePosterior(memory_, likelihood)); ASSERT_TRUE(filterDense.computePosterior(memory_, likelihood));
ASSERT_TRUE(filterSparse.computePosterior(memory_, likelihood)); ASSERT_TRUE(filterSparse.computePosterior(memory_, likelihood));
ASSERT_FALSE(filterSparse.isPredictionSparse());
const cv::Mat prediction = filterSparse.generatePrediction(memory_, ids); const cv::Mat prediction = filterSparse.generatePrediction(memory_, ids);
ASSERT_EQ(prediction.rows, (int)ids.size()); ASSERT_EQ(prediction.rows, (int)ids.size());
ASSERT_EQ(prediction.cols, (int)ids.size()); ASSERT_EQ(prediction.cols, (int)ids.size());
@@ -1383,7 +1461,7 @@ TEST_F(BayesFilterMemoryFixture, PredictionFallsBackToTheMatrixWhenTheGraphBecom
ids = getBayesIds(); ids = getBayesIds();
ASSERT_TRUE(filter.computePosterior(memory_, uniformLikelihood(ids))); ASSERT_TRUE(filter.computePosterior(memory_, uniformLikelihood(ids)));
} }
ASSERT_TRUE(filter.generatePrediction(memory_, ids).empty()); ASSERT_TRUE(filter.isPredictionSparse());
const cv::Mat infMatrix = cv::Mat::eye(6, 6, CV_64FC1); const cv::Mat infMatrix = cv::Mat::eye(6, 6, CV_64FC1);
const size_t third = ids.size()/3; const size_t third = ids.size()/3;
@@ -1406,6 +1484,7 @@ TEST_F(BayesFilterMemoryFixture, PredictionFallsBackToTheMatrixWhenTheGraphBecom
// The matrix is built for the locations of this iteration, not updated against the ones it // The matrix is built for the locations of this iteration, not updated against the ones it
// was built for before the sparse form took over. // was built for before the sparse form took over.
ASSERT_FALSE(filter.isPredictionSparse());
const cv::Mat prediction = filter.generatePrediction(memory_, fewerIds); const cv::Mat prediction = filter.generatePrediction(memory_, fewerIds);
ASSERT_EQ(prediction.rows, (int)fewerIds.size()); ASSERT_EQ(prediction.rows, (int)fewerIds.size());
ASSERT_EQ(prediction.cols, (int)fewerIds.size()); ASSERT_EQ(prediction.cols, (int)fewerIds.size());
@@ -1421,14 +1500,14 @@ TEST_F(BayesFilterMemoryFixture, PredictionFallsBackToTheMatrixWhenTheGraphBecom
EXPECT_NEAR(sum, 1.0f, 1e-4f); EXPECT_NEAR(sum, 1.0f, 1e-4f);
} }
// generatePrediction() hands over the prediction matrix, which only the dense mode has. // generatePrediction() hands over the prediction whichever form it is held in, expanding the
// With the prediction kept sparse there is none, and it says so with an empty matrix rather // sparse form into a matrix for the caller. Rtabmap::dumpPrediction() reads it that way, and
// than building one: Rtabmap::dumpPrediction(), the one caller outside the filter, reports // so does anyone comparing the two forms: they are built through the same column arithmetic,
// that the matrix cannot be exported instead of spending the memory and the time that // so the matrices have to come out equal, value for value.
// keeping the prediction sparse is saving. TEST_F(BayesFilterMemoryFixture, GeneratePredictionExpandsTheSparseFormIntoTheSameMatrix)
TEST_F(BayesFilterMemoryFixture, GeneratePredictionIsEmptyWhileThePredictionIsKeptSparse)
{ {
addChain(20); // Large enough for the sparse form to be worth keeping, so that it is the one answering.
addChain(40);
ParametersMap paramsDense; ParametersMap paramsDense;
paramsDense.insert(ParametersPair(Parameters::kBayesPredictionLC(), kPredictionNewPlace10Stay50Neighbor25_15)); paramsDense.insert(ParametersPair(Parameters::kBayesPredictionLC(), kPredictionNewPlace10Stay50Neighbor25_15));
@@ -1442,12 +1521,29 @@ TEST_F(BayesFilterMemoryFixture, GeneratePredictionIsEmptyWhileThePredictionIsKe
const std::vector<int> ids = getBayesIds(); const std::vector<int> ids = getBayesIds();
const std::map<int, float> likelihood = uniformLikelihood(ids); const std::map<int, float> likelihood = uniformLikelihood(ids);
filterDense.computePosterior(memory_, likelihood); ASSERT_TRUE(filterDense.computePosterior(memory_, likelihood));
filterSparse.computePosterior(memory_, likelihood); ASSERT_TRUE(filterSparse.computePosterior(memory_, likelihood));
ASSERT_FALSE(filterDense.isPredictionSparse());
ASSERT_TRUE(filterSparse.isPredictionSparse());
const cv::Mat predictionDense = filterDense.generatePrediction(memory_, ids); const cv::Mat predictionDense = filterDense.generatePrediction(memory_, ids);
const cv::Mat predictionSparse = filterSparse.generatePrediction(memory_, ids);
ASSERT_EQ(predictionDense.rows, (int)ids.size()); ASSERT_EQ(predictionDense.rows, (int)ids.size());
ASSERT_EQ(predictionDense.cols, (int)ids.size()); ASSERT_EQ(predictionDense.cols, (int)ids.size());
ASSERT_EQ(predictionSparse.rows, predictionDense.rows);
ASSERT_EQ(predictionSparse.cols, predictionDense.cols);
EXPECT_TRUE(filterSparse.generatePrediction(memory_, ids).empty()); for(int col = 0; col < predictionDense.cols; ++col)
{
for(int row = 0; row < predictionDense.rows; ++row)
{
EXPECT_FLOAT_EQ(predictionAt(predictionSparse, row, col),
predictionAt(predictionDense, row, col))
<< "row=" << row << " col=" << col;
}
}
// The expansion is handed over, not kept: the sparse form is still the one held.
EXPECT_TRUE(filterSparse.isPredictionSparse());
EXPECT_LT(filterSparse.getMemoryUsed(), filterDense.getMemoryUsed());
} }