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;
/**
* @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.
* @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
* 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 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
* @ref Parameters::kBayesSparsePrediction() is enabled and the prediction is being
* kept in its sparse form, in which case no matrix exists to return.
* @return Square CV_32FC1 matrix of size ids.size() × ids.size().
*/
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();
}
bool BayesFilter::isPredictionSparse() const
{
return !_sparse->empty();
}
void BayesFilter::reset()
{
_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)
{
if(!_sparse->empty())
if(!_sparse->empty() && _sparse->ids() == ids)
{
// The prediction is being kept sparse, so there is no matrix to return.
return cv::Mat();
// Expanded from the sparse form, which holds the same prediction. The matrix costs
// 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)
{
-9
View File
@@ -5820,15 +5820,6 @@ void Rtabmap::dumpPrediction() const
}
}
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;
std::string fileName = this->getWorkingDir() + "/DumpPrediction.txt";
+19
View File
@@ -42,6 +42,25 @@ void SparsePrediction::clear()
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
{
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 <opencv2/core/core.hpp>
#include <utility>
#include <vector>
@@ -87,6 +88,14 @@ public:
/// prior = prediction x posterior.
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;
private:
+137 -41
View File
@@ -261,6 +261,8 @@ protected:
ParametersMap params;
params.insert(ParametersPair(Parameters::kBayesPredictionLC(), kPredictionNewPlace10Stay50Neighbor25_15));
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);
std::vector<int> ids = getBayesIds();
@@ -286,6 +288,40 @@ protected:
// 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)
{
BayesFilter filter;
@@ -376,10 +412,10 @@ TEST(BayesFilterTest, GetPredictionLCStrRoundTrip)
// reset Tests
TEST(BayesFilterTest, Reset)
TEST_P(BayesFilterModeTest, Reset)
{
Memory memory;
ParametersMap params;
ParametersMap params = modeParams();
params.insert(ParametersPair(Parameters::kBayesPredictionLC(), kPredictionNewPlace10Stay90));
BayesFilter filter(params);
@@ -394,9 +430,9 @@ TEST(BayesFilterTest, Reset)
// getMemoryUsed Tests
TEST(BayesFilterTest, GetMemoryUsed)
TEST_P(BayesFilterModeTest, GetMemoryUsed)
{
BayesFilter filter;
BayesFilter filter(modeParams());
EXPECT_GE(filter.getMemoryUsed(), sizeof(BayesFilter));
Memory memory;
@@ -430,9 +466,11 @@ TEST(BayesFilterTest, ComputePosteriorEmptyLikelihood)
// 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"));
BayesFilter filter(params);
@@ -468,9 +506,9 @@ TEST(BayesFilterTest, GeneratePredictionCachedWhenIdsUnchanged)
EXPECT_EQ(prediction1.data, prediction2.data);
}
TEST(BayesFilterTest, ComputePosteriorVirtualPlaceOnly)
TEST_P(BayesFilterModeTest, ComputePosteriorVirtualPlaceOnly)
{
ParametersMap params;
ParametersMap params = modeParams();
params.insert(ParametersPair(Parameters::kBayesPredictionLC(), kPredictionNewPlace10Stay90));
BayesFilter filter(params);
@@ -486,9 +524,9 @@ TEST(BayesFilterTest, ComputePosteriorVirtualPlaceOnly)
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));
BayesFilter filter(params);
@@ -503,9 +541,9 @@ TEST(BayesFilterTest, ComputePosteriorNormalizesPosterior)
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));
BayesFilter filter(params);
@@ -524,6 +562,10 @@ TEST(BayesFilterTest, ComputePosteriorUpdatesWithNewLikelihood)
// 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)
{
addChain(5);
@@ -534,6 +576,7 @@ TEST_F(BayesFilterMemoryFixture, GeneratePredictionLinearChain)
ParametersMap params;
params.insert(ParametersPair(Parameters::kBayesPredictionLC(), kPredictionNewPlace10Stay50Neighbor25_15));
params.insert(ParametersPair(Parameters::kBayesVirtualPlacePriorThr(), "0.9"));
params.insert(ParametersPair(Parameters::kBayesSparsePrediction(), "false"));
BayesFilter filter(params);
const std::vector<int> ids = getBayesIds();
@@ -592,11 +635,14 @@ TEST_F(BayesFilterMemoryFixture, GeneratePredictionLinearChain)
(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);
ParametersMap params;
ParametersMap params = modeParams();
params.insert(ParametersPair(Parameters::kBayesPredictionLC(), kPredictionSumBelowOne));
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[0], Memory::kIdVirtual);
ASSERT_TRUE(filter.computePosterior(memory_, uniformLikelihood(ids)));
ASSERT_FALSE(filter.isPredictionSparse());
const std::vector<double> & predictionLC = filter.getPredictionLC();
ASSERT_EQ(predictionLC.size(), 2u);
@@ -647,29 +696,54 @@ TEST_F(BayesFilterMemoryFixture, GeneratePredictionNormalizesWhenSumBelowOne)
(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));
BayesFilter filter(params);
const std::vector<int> ids = getBayesIds();
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 prediction2 = filter.generatePrediction(memory_, ids);
ASSERT_FALSE(prediction1.empty());
EXPECT_EQ(prediction1.data, prediction2.data);
ASSERT_EQ(prediction1.rows, (int)ids.size());
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);
ParametersMap params;
ParametersMap params = modeParams();
params.insert(ParametersPair(Parameters::kBayesPredictionLC(), kPredictionNewPlace10Stay50Neighbor25_15));
BayesFilter filter(params);
@@ -709,11 +783,11 @@ TEST_F(BayesFilterMemoryFixture, ComputePosteriorNormalizedWithGraph)
EXPECT_NEAR(sum, 1.0f, 1e-4f);
}
TEST_F(BayesFilterMemoryFixture, ComputePosteriorFavorsHighLikelihood)
TEST_P(BayesFilterMemoryModeFixture, ComputePosteriorFavorsHighLikelihood)
{
addChain(16);
ParametersMap params;
ParametersMap params = modeParams();
params.insert(ParametersPair(Parameters::kBayesPredictionLC(), kPredictionNewPlace10Stay70Neighbor20));
BayesFilter filter(params);
@@ -772,11 +846,11 @@ TEST_F(BayesFilterMemoryFixture, ComputePosteriorFavorsHighLikelihood)
}
}
TEST_F(BayesFilterMemoryFixture, ComputePosteriorSequentialIterations)
TEST_P(BayesFilterMemoryModeFixture, ComputePosteriorSequentialIterations)
{
initMemory(5);
ParametersMap params;
ParametersMap params = modeParams();
params.insert(ParametersPair(Parameters::kBayesPredictionLC(), kPredictionNewPlace10Stay50Neighbor25_15));
params.insert(ParametersPair(Parameters::kBayesFullPredictionUpdate(), "true"));
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
// (not the odometry neighbor link to the node added in the previous iteration).
initMemory(5);
ParametersMap params;
ParametersMap params = modeParams();
params.insert(ParametersPair(Parameters::kBayesPredictionLC(), kPredictionNewPlace10Stay50Neighbor25_15));
params.insert(ParametersPair(Parameters::kBayesFullPredictionUpdate(), "true"));
BayesFilter filter(params);
@@ -915,6 +989,8 @@ TEST_F(BayesFilterMemoryFixture, CompareFullPredictionUpdateModes)
ParametersMap paramsIncremental;
paramsIncremental.insert(ParametersPair(Parameters::kBayesPredictionLC(), kPredictionNewPlace10Stay50Neighbor25_15));
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;
paramsFull[Parameters::kBayesFullPredictionUpdate()] = "true";
@@ -989,6 +1065,7 @@ TEST_F(BayesFilterMemoryFixture, FullPredictionUpdateRegeneratesMatrix)
ParametersMap params;
params.insert(ParametersPair(Parameters::kBayesPredictionLC(), kPredictionNewPlace10Stay70Neighbor20));
params.insert(ParametersPair(Parameters::kBayesSparsePrediction(), "false"));
auto expectMatrixGrowsOnNewNode = [&](bool fullPredictionUpdate) {
params[Parameters::kBayesFullPredictionUpdate()] = fullPredictionUpdate ? "true" : "false";
@@ -1332,8 +1409,8 @@ TEST_F(BayesFilterMemoryFixture, PredictionCrossesFromSparseBackToTheMatrix)
<< "iter=" << iter << " id=" << ids[i];
}
}
// Grown into the sparse form: there is no matrix left to hand over.
ASSERT_TRUE(filterSparse.generatePrediction(memory_, ids).empty());
// Grown into the sparse form, so the matrix of the first iterations is gone.
ASSERT_TRUE(filterSparse.isPredictionSparse());
// 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.
@@ -1351,6 +1428,7 @@ TEST_F(BayesFilterMemoryFixture, PredictionCrossesFromSparseBackToTheMatrix)
ASSERT_TRUE(filterDense.computePosterior(memory_, likelihood));
ASSERT_TRUE(filterSparse.computePosterior(memory_, likelihood));
ASSERT_FALSE(filterSparse.isPredictionSparse());
const cv::Mat prediction = filterSparse.generatePrediction(memory_, ids);
ASSERT_EQ(prediction.rows, (int)ids.size());
ASSERT_EQ(prediction.cols, (int)ids.size());
@@ -1383,7 +1461,7 @@ TEST_F(BayesFilterMemoryFixture, PredictionFallsBackToTheMatrixWhenTheGraphBecom
ids = getBayesIds();
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 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
// was built for before the sparse form took over.
ASSERT_FALSE(filter.isPredictionSparse());
const cv::Mat prediction = filter.generatePrediction(memory_, fewerIds);
ASSERT_EQ(prediction.rows, (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);
}
// generatePrediction() hands over the prediction matrix, which only the dense mode has.
// With the prediction kept sparse there is none, and it says so with an empty matrix rather
// than building one: Rtabmap::dumpPrediction(), the one caller outside the filter, reports
// that the matrix cannot be exported instead of spending the memory and the time that
// keeping the prediction sparse is saving.
TEST_F(BayesFilterMemoryFixture, GeneratePredictionIsEmptyWhileThePredictionIsKeptSparse)
// generatePrediction() hands over the prediction whichever form it is held in, expanding the
// sparse form into a matrix for the caller. Rtabmap::dumpPrediction() reads it that way, and
// so does anyone comparing the two forms: they are built through the same column arithmetic,
// so the matrices have to come out equal, value for value.
TEST_F(BayesFilterMemoryFixture, GeneratePredictionExpandsTheSparseFormIntoTheSameMatrix)
{
addChain(20);
// Large enough for the sparse form to be worth keeping, so that it is the one answering.
addChain(40);
ParametersMap paramsDense;
paramsDense.insert(ParametersPair(Parameters::kBayesPredictionLC(), kPredictionNewPlace10Stay50Neighbor25_15));
@@ -1442,12 +1521,29 @@ TEST_F(BayesFilterMemoryFixture, GeneratePredictionIsEmptyWhileThePredictionIsKe
const std::vector<int> ids = getBayesIds();
const std::map<int, float> likelihood = uniformLikelihood(ids);
filterDense.computePosterior(memory_, likelihood);
filterSparse.computePosterior(memory_, likelihood);
ASSERT_TRUE(filterDense.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 predictionSparse = filterSparse.generatePrediction(memory_, ids);
ASSERT_EQ(predictionDense.rows, (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());
}