refactoring

This commit is contained in:
matlabbe
2026-08-19 16:41:56 -07:00
parent ed5bd74b48
commit eca1e53e95
5 changed files with 127 additions and 86 deletions
+16 -9
View File
@@ -95,12 +95,14 @@ public:
* The prediction matrix is generated or updated from @ref Memory using the ids present
* in @p likelihood.
*
* Read the result with @ref getPosteriorIds() and @ref getPosteriorValues().
*
* @param memory Working memory instance (must not be null).
* @param likelihood Observation likelihood per signature id (must not be empty).
* @return Reference to the internal posterior map (id → probability). On error (null
* memory, empty likelihood, or invalid prediction model), returns the unchanged posterior.
* @return False on error (null memory, empty likelihood, or invalid prediction model),
* the posterior being left unchanged.
*/
const std::map<int, float> & computePosterior(const Memory * memory, const std::map<int, float> & likelihood);
bool computePosterior(const Memory * memory, const std::map<int, float> & likelihood);
/**
* @brief Clears posterior, prediction matrix and cached neighbor indices.
@@ -123,10 +125,16 @@ public:
void setPredictionLC(const std::string & prediction);
/**
* @brief Returns the current posterior probability map.
* @return Map of signature id to normalized posterior probability. This is the probability to be at the given location.
* @brief The locations the posterior is over, ascending by id.
*
* The virtual place (@ref Memory::kIdVirtual) is the first of them when it is one.
*/
const std::map<int, float> & getPosterior() const {return _posterior;}
const std::vector<int> & getPosteriorIds() const {return _posteriorIds;}
/**
* @brief The probability of each location of @ref getPosteriorIds(), in the same order.
*/
const std::vector<float> & getPosteriorValues() const {return _posteriorValues;}
/**
* @brief Returns the virtual place prior threshold.
@@ -286,9 +294,8 @@ private:
void multiplySparsePrediction(const std::vector<float> & posterior, std::vector<float> & prior) const;
private:
std::map<int, float> _posterior; ///< Current posterior (signature id → probability).
std::vector<int> _posteriorIds; ///< The ids of _posterior, in its order.
std::vector<float> _posteriorValues; ///< The probabilities of _posterior, in its order: what an iteration works on.
std::vector<int> _posteriorIds; ///< The locations the posterior is over, ascending by id.
std::vector<float> _posteriorValues; ///< The probability of each of them, in the same order.
std::vector<int> _likelihoodIds; ///< The ids of the likelihood of an iteration, in its order.
std::vector<float> _likelihoodValues; ///< The likelihood of an iteration, in the same order.
std::vector<float> _priorValues; ///< The prior of an iteration, in the same order.
+29 -34
View File
@@ -166,7 +166,6 @@ bool BayesFilter::posteriorHasSameIds(const std::vector<int> & ids) const
void BayesFilter::reset()
{
_posterior.clear();
_posteriorIds.clear();
_posteriorValues.clear();
_prediction = cv::Mat();
@@ -176,26 +175,26 @@ void BayesFilter::reset()
_neighborsIndex.clear();
}
const std::map<int, float> & BayesFilter::computePosterior(const Memory * memory, const std::map<int, float> & likelihood)
bool BayesFilter::computePosterior(const Memory * memory, const std::map<int, float> & likelihood)
{
ULOGGER_DEBUG("");
if(!memory)
{
ULOGGER_ERROR("Memory is Null!");
return _posterior;
return false;
}
if(!likelihood.size())
{
ULOGGER_ERROR("likelihood is empty!");
return _posterior;
return false;
}
if(_predictionLC.size() < 2)
{
ULOGGER_ERROR("Prediction is not valid!");
return _posterior;
return false;
}
UTimer timer;
@@ -282,7 +281,7 @@ const std::map<int, float> & BayesFilter::computePosterior(const Memory * memory
this->updatePosterior(memory, likelihood);
}
UASSERT(_posteriorValues.size() == likelihood.size());
ULOGGER_DEBUG("STEP1-update posterior=%fs, _posterior size=%d", timer.ticks(), (int)_posterior.size());
ULOGGER_DEBUG("STEP1-update posterior=%fs, posterior size=%d", timer.ticks(), (int)_posteriorValues.size());
// Multiply prediction matrix with the last posterior
// (m,m) X (m,1) = (m,1)
@@ -327,19 +326,8 @@ const std::map<int, float> & BayesFilter::computePosterior(const Memory * memory
}
}
// The posterior the caller reads is the map, which is filled from the values in one
// walk of it: its ids are the ones of the likelihood, in the same order.
{
size_t k = 0;
for(std::map<int, float>::iterator iter=_posterior.begin(); iter!=_posterior.end(); ++iter)
{
iter->second = _posteriorValues[k++];
}
}
ULOGGER_DEBUG("normalize time=%fs", timer.ticks());
//std::cout << "Posterior=" << _posterior << std::endl;
return _posterior;
return true;
}
// A column of the prediction matrix, given as a pointer to its first value and the
@@ -428,11 +416,10 @@ cv::Mat BayesFilter::generatePrediction(const Memory * memory, const std::vector
{
return _prediction;
}
std::vector<int> oldIds = uKeys(_posterior);
if(!_fullPredictionUpdate && !_prediction.empty())
{
return updatePrediction(_prediction, memory, oldIds, ids);
// The ids the matrix was built for, which the posterior is still indexed by.
return updatePrediction(_prediction, memory, _posteriorIds, ids);
}
UDEBUG("");
@@ -889,7 +876,6 @@ void BayesFilter::multiplySparsePrediction(const std::vector<float> & posterior,
unsigned long BayesFilter::getMemoryUsed() const
{
long memoryUsage = sizeof(BayesFilter);
memoryUsage += _posterior.size() * (sizeof(float)+sizeof(int)+sizeof(std::map<int, float>::iterator)) + sizeof(std::map<int, float>);
if(!_prediction.empty())
{
memoryUsage += _prediction.total() * _prediction.elemSize();
@@ -1231,25 +1217,34 @@ cv::Mat BayesFilter::updatePrediction(const cv::Mat & oldPrediction,
// Realigns the posterior with the ids of the likelihood, keeping the probability of the
// locations that are in both. Called only when they differ, which over a fixed graph never
// happens after the first iteration.
//
// Both are sorted by id, so the ones in both are found by walking them side by side rather
// than searching for each.
void BayesFilter::updatePosterior(const Memory * memory, const std::map<int, float> & likelihood)
{
ULOGGER_DEBUG("");
const bool wasEmpty = _posterior.empty();
std::map<int, float> newPosterior;
_posteriorIds.clear();
_posteriorIds.reserve(likelihood.size());
_posteriorValues.resize(likelihood.size());
const bool wasEmpty = _posteriorIds.empty();
std::vector<int> ids;
std::vector<float> values;
ids.reserve(likelihood.size());
values.reserve(likelihood.size());
size_t k = 0;
for(std::map<int, float>::const_iterator iter=likelihood.begin(); iter!=likelihood.end(); ++iter)
{
const std::map<int, float>::const_iterator post = _posterior.find(iter->first);
const float value = post != _posterior.end() ? post->second : (wasEmpty ? 1.0f : 0.0f);
// The ids come from a map, so they arrive sorted and each one belongs at the end.
newPosterior.insert(newPosterior.end(), std::make_pair(iter->first, value));
_posteriorIds.push_back(iter->first);
_posteriorValues[k++] = value;
while(k < _posteriorIds.size() && _posteriorIds[k] < iter->first)
{
++k;
}
float value = wasEmpty ? 1.0f : 0.0f;
if(k < _posteriorIds.size() && _posteriorIds[k] == iter->first)
{
value = _posteriorValues[k];
}
ids.push_back(iter->first);
values.push_back(value);
}
_posterior = newPosterior;
_posteriorIds.swap(ids);
_posteriorValues.swap(values);
}
} // namespace rtabmap
+22 -11
View File
@@ -1258,8 +1258,6 @@ bool Rtabmap::process(
std::map<int, float> adjustedLikelihood;
std::map<int, float> likelihood;
std::map<int, int> weights;
const std::map<int, float> emptyPosterior;
const std::map<int, float> * posterior = &emptyPosterior;
std::list<std::pair<int, float> > reactivateHypotheses;
std::map<int, int> childCount;
@@ -2139,7 +2137,7 @@ bool Rtabmap::process(
ULOGGER_INFO("getting posterior...");
// Compute the posterior
posterior = &_bayesFilter->computePosterior(_memory, likelihood);
_bayesFilter->computePosterior(_memory, likelihood);
timePosteriorCalculation = timer.ticks();
ULOGGER_INFO("timePosteriorCalculation=%fs",timePosteriorCalculation);
@@ -2153,17 +2151,21 @@ bool Rtabmap::process(
// Select the highest hypothesis
//============================================================
ULOGGER_INFO("creating hypotheses...");
if(posterior->size())
const std::vector<int> & posteriorIds = _bayesFilter->getPosteriorIds();
const std::vector<float> & posteriorValues = _bayesFilter->getPosteriorValues();
if(posteriorIds.size())
{
for(std::map<int, float>::const_reverse_iterator iter = posterior->rbegin(); iter != posterior->rend(); ++iter)
// Highest id first, to keep the same one on equal probabilities as when this
// walked a map backwards.
for(size_t i=posteriorIds.size(); i-- > 0;)
{
if(iter->first > 0 && iter->second > _highestHypothesis.second)
if(posteriorIds[i] > 0 && posteriorValues[i] > _highestHypothesis.second)
{
_highestHypothesis = *iter;
_highestHypothesis = std::make_pair(posteriorIds[i], posteriorValues[i]);
}
}
// With the virtual place, use sum of LC probabilities (1 - virtual place hypothesis).
_highestHypothesis.second = 1-posterior->begin()->second;
_highestHypothesis.second = 1-posteriorValues[0];
}
timeHypothesesCreation = timer.ticks();
ULOGGER_INFO("Highest hypothesis=%d, value=%f, timeHypothesesCreation=%fs", _highestHypothesis.first, _highestHypothesis.second, timeHypothesesCreation);
@@ -2194,7 +2196,7 @@ bool Rtabmap::process(
if(_highestHypothesis.second >= loopThr)
{
rejectedLoopClosure = true;
if(posterior->size() <= 2 && loopThr>0.0f)
if(_bayesFilter->getPosteriorIds().size() <= 2 && loopThr>0.0f)
{
// Ignore loop closure if there is only one loop closure hypothesis
UDEBUG("rejected hypothesis: single hypothesis");
@@ -4195,7 +4197,9 @@ bool Rtabmap::process(
}
// Posterior is empty if a bad signature is detected
float vpHypothesis = posterior->size()?posterior->at(Memory::kIdVirtual):0.0f;
// The virtual place is the first location of the posterior when it is one of them.
const std::vector<int> & vpIds = _bayesFilter->getPosteriorIds();
float vpHypothesis = (vpIds.size() && vpIds[0]==Memory::kIdVirtual)?_bayesFilter->getPosteriorValues()[0]:0.0f;
int loopId = _loopClosureHypothesis.first>0?_loopClosureHypothesis.first:lastProximitySpaceClosureId;
// prepare statistics
@@ -4412,7 +4416,14 @@ bool Rtabmap::process(
statistics_.setWeights(weights);
if(_publishPdf)
{
statistics_.setPosterior(*posterior);
const std::vector<int> & ids = _bayesFilter->getPosteriorIds();
const std::vector<float> & values = _bayesFilter->getPosteriorValues();
std::map<int, float> posterior;
for(size_t i=0; i<ids.size(); ++i)
{
posterior.insert(posterior.end(), std::make_pair(ids[i], values[i]));
}
statistics_.setPosterior(posterior);
}
if(_publishLikelihood)
{
+16 -2
View File
@@ -342,6 +342,20 @@ void printGraph(const std::string & path, const RealGraph & graph, int removedLi
std::cout << " (" << graph.skippedLinks << " on a single location or on a landmark not rebuilt, "
<< removedLinks << " gaps in the chain)" << std::endl;
}
// The posterior as a map, which the filter no longer builds: it holds the locations and
// their probabilities as two vectors, and a test reads them more easily as a map.
static std::map<int, float> posteriorOf(const BayesFilter & filter)
{
const std::vector<int> & ids = filter.getPosteriorIds();
const std::vector<float> & values = filter.getPosteriorValues();
std::map<int, float> posterior;
for(size_t i = 0; i < ids.size(); ++i)
{
posterior.insert(posterior.end(), std::make_pair(ids[i], values[i]));
}
return posterior;
}
struct Result
{
double firstIteration = 0.0; // includes generating the prediction, sparse or dense
@@ -396,7 +410,7 @@ Result run(const Memory * memory,
}
result.steadyState = best > 0.0 ? best : result.firstIteration;
result.memoryUsed = filter.getMemoryUsed();
result.posterior = filter.getPosterior();
result.posterior = posteriorOf(filter);
return result;
}
@@ -709,7 +723,7 @@ TEST(BayesFilterPerfTest, DenseVsSparsePredictionWhileMappingARealSession)
if(best == 0.0 || elapsed < best) best = elapsed;
if(elapsed > worst) worst = elapsed;
}
lastPosterior[sparse] = filter.getPosterior();
lastPosterior[sparse] = posteriorOf(filter);
printf("[ ] %-6s first iteration %8.1f ms, then per added location: "
"fastest %8.1f ms, mean %8.1f ms, slowest %8.1f ms, filter memory %7.1f MB\n",
+44 -30
View File
@@ -28,6 +28,20 @@ static const char kPredictionNewPlace10Stay70Neighbor20[] = "0.1 0.7 0.2"; /
static const char kPredictionNewPlace10Stay50Neighbor25_15[] = "0.1 0.5 0.25 0.15"; // sum = 1
static const char kPredictionSumBelowOne[] = "0.1 0.5"; // sum = 0.6 (non-default, tests normalization)
// The posterior as a map, which the filter no longer builds: it holds the locations and
// their probabilities as two vectors, and a test reads them more easily as a map.
static std::map<int, float> posteriorOf(const BayesFilter & filter)
{
const std::vector<int> & ids = filter.getPosteriorIds();
const std::vector<float> & values = filter.getPosteriorValues();
std::map<int, float> posterior;
for(size_t i = 0; i < ids.size(); ++i)
{
posterior.insert(posterior.end(), std::make_pair(ids[i], values[i]));
}
return posterior;
}
static bool approxEqual(float a, float b, float epsilon = 1e-5f)
{
return std::fabs(a - b) < epsilon;
@@ -262,7 +276,7 @@ protected:
likelihood = uniformLikelihood(ids);
filter.computePosterior(memory_, likelihood);
return std::make_pair(filter.getPosterior(), filter.generatePrediction(memory_, ids));
return std::make_pair(posteriorOf(filter), filter.generatePrediction(memory_, ids));
}
Memory * memory_ = nullptr;
@@ -276,7 +290,7 @@ TEST(BayesFilterTest, DefaultConstructor)
{
BayesFilter filter;
EXPECT_TRUE(filter.getPosterior().empty());
EXPECT_TRUE(posteriorOf(filter).empty());
EXPECT_FLOAT_EQ(filter.getVirtualPlacePrior(), Parameters::defaultBayesVirtualPlacePriorThr());
EXPECT_GE(filter.getPredictionLC().size(), 2u);
EXPECT_FALSE(filter.getPredictionLCStr().empty());
@@ -372,10 +386,10 @@ TEST(BayesFilterTest, Reset)
std::map<int, float> likelihood;
likelihood[Memory::kIdVirtual] = 1.0f;
filter.computePosterior(&memory, likelihood);
ASSERT_FALSE(filter.getPosterior().empty());
ASSERT_FALSE(posteriorOf(filter).empty());
filter.reset();
EXPECT_TRUE(filter.getPosterior().empty());
EXPECT_TRUE(posteriorOf(filter).empty());
}
// getMemoryUsed Tests
@@ -400,8 +414,8 @@ TEST(BayesFilterTest, ComputePosteriorNullMemory)
std::map<int, float> likelihood;
likelihood[1] = 1.0f;
const std::map<int, float> & result = filter.computePosterior(nullptr, likelihood);
EXPECT_TRUE(result.empty());
EXPECT_FALSE(filter.computePosterior(nullptr, likelihood));
EXPECT_TRUE(posteriorOf(filter).empty());
}
TEST(BayesFilterTest, ComputePosteriorEmptyLikelihood)
@@ -410,8 +424,8 @@ TEST(BayesFilterTest, ComputePosteriorEmptyLikelihood)
BayesFilter filter;
std::map<int, float> likelihood;
const std::map<int, float> & result = filter.computePosterior(&memory, likelihood);
EXPECT_TRUE(result.empty());
EXPECT_FALSE(filter.computePosterior(&memory, likelihood));
EXPECT_TRUE(posteriorOf(filter).empty());
}
// generatePrediction Tests (virtual place only, no graph)
@@ -466,7 +480,7 @@ TEST(BayesFilterTest, ComputePosteriorVirtualPlaceOnly)
filter.computePosterior(&memory, likelihood);
const std::map<int, float> & posterior = filter.getPosterior();
const std::map<int, float> & posterior = posteriorOf(filter);
ASSERT_EQ(posterior.size(), 1u);
EXPECT_EQ(posterior.begin()->first, Memory::kIdVirtual);
EXPECT_TRUE(approxEqual(posterior.begin()->second, 1.0f));
@@ -484,7 +498,7 @@ TEST(BayesFilterTest, ComputePosteriorNormalizesPosterior)
filter.computePosterior(&memory, likelihood);
const std::map<int, float> & posterior = filter.getPosterior();
const std::map<int, float> & posterior = posteriorOf(filter);
ASSERT_EQ(posterior.size(), 1u);
EXPECT_TRUE(approxEqual(posterior.begin()->second, 1.0f));
}
@@ -503,7 +517,7 @@ TEST(BayesFilterTest, ComputePosteriorUpdatesWithNewLikelihood)
likelihood[Memory::kIdVirtual] = 0.5f;
filter.computePosterior(&memory, likelihood);
const std::map<int, float> & posterior = filter.getPosterior();
const std::map<int, float> & posterior = posteriorOf(filter);
ASSERT_EQ(posterior.size(), 1u);
EXPECT_TRUE(approxEqual(posterior.begin()->second, 1.0f));
}
@@ -666,7 +680,7 @@ TEST_F(BayesFilterMemoryFixture, ComputePosteriorNormalizedWithGraph)
std::map<int, float> likelihood = uniformLikelihood(ids);
filter.computePosterior(memory_, likelihood);
const std::map<int, float> & posterior = filter.getPosterior();
const std::map<int, float> & posterior = posteriorOf(filter);
ASSERT_EQ(posterior.size(), ids.size());
// Linear chain ids: [virtual, 1..15]. Uniform likelihood, init posterior on virtual place.
@@ -738,7 +752,7 @@ TEST_F(BayesFilterMemoryFixture, ComputePosteriorFavorsHighLikelihood)
{
filter.computePosterior(memory_, likelihood);
const std::map<int, float> & posterior = filter.getPosterior();
const std::map<int, float> & posterior = posteriorOf(filter);
ASSERT_EQ(posterior.size(), ids.size());
for(size_t i = 0; i < ids.size(); ++i)
@@ -803,7 +817,7 @@ TEST_F(BayesFilterMemoryFixture, ComputePosteriorSequentialIterations)
filter.computePosterior(memory_, likelihood);
const std::map<int, float> & posterior = filter.getPosterior();
const std::map<int, float> & posterior = posteriorOf(filter);
ASSERT_EQ(posterior.size(), ids.size());
ASSERT_GE(iter, 5);
@@ -860,7 +874,7 @@ TEST_F(BayesFilterMemoryFixture, ComputePosteriorSequentialIterationsWithLoopClo
filter.computePosterior(memory_, likelihood);
const std::map<int, float> & posterior = filter.getPosterior();
const std::map<int, float> & posterior = posteriorOf(filter);
ASSERT_EQ(posterior.size(), ids.size());
ASSERT_GE(iter, 5);
@@ -918,8 +932,8 @@ TEST_F(BayesFilterMemoryFixture, CompareFullPredictionUpdateModes)
filterIncremental.computePosterior(memory_, likelihood);
filterFull.computePosterior(memory_, likelihood);
const std::map<int, float> & posteriorIncremental = filterIncremental.getPosterior();
const std::map<int, float> & posteriorFull = filterFull.getPosterior();
const std::map<int, float> & posteriorIncremental = posteriorOf(filterIncremental);
const std::map<int, float> & posteriorFull = posteriorOf(filterFull);
ASSERT_EQ(posteriorIncremental.size(), posteriorFull.size());
for(size_t i = 0; i < ids.size(); ++i)
{
@@ -1038,8 +1052,8 @@ TEST_F(BayesFilterMemoryFixture, CompareSparsePredictionModes)
filterDense.computePosterior(memory_, likelihood);
filterSparse.computePosterior(memory_, likelihood);
const std::map<int, float> & posteriorDense = filterDense.getPosterior();
const std::map<int, float> & posteriorSparse = filterSparse.getPosterior();
const std::map<int, float> & posteriorDense = posteriorOf(filterDense);
const std::map<int, float> & posteriorSparse = posteriorOf(filterSparse);
ASSERT_EQ(posteriorDense.size(), posteriorSparse.size());
for(size_t i = 0; i < ids.size(); ++i)
{
@@ -1071,11 +1085,11 @@ TEST_F(BayesFilterMemoryFixture, SparsePredictionFallsBackWhenModelSumsBelowOne)
filterDense.computePosterior(memory_, likelihood);
filterSparse.computePosterior(memory_, likelihood);
ASSERT_EQ(filterDense.getPosterior().size(), filterSparse.getPosterior().size());
ASSERT_EQ(posteriorOf(filterDense).size(), posteriorOf(filterSparse).size());
for(size_t i = 0; i < ids.size(); ++i)
{
EXPECT_NEAR(filterDense.getPosterior().at(ids[i]),
filterSparse.getPosterior().at(ids[i]), 1e-5f) << "id=" << ids[i];
EXPECT_NEAR(posteriorOf(filterDense).at(ids[i]),
posteriorOf(filterSparse).at(ids[i]), 1e-5f) << "id=" << ids[i];
}
// Falling back means the sparse view was left empty, so no extra memory is used.
@@ -1148,7 +1162,7 @@ TEST_F(BayesFilterMemoryFixture, SparsePredictionIsExactOnASingleColumn)
}
filterDense.computePosterior(memory_, oneHot);
filterSparse.computePosterior(memory_, oneHot);
ASSERT_FLOAT_EQ(filterDense.getPosterior().at(ids[selected]), 1.0f) << "column=" << selected;
ASSERT_FLOAT_EQ(posteriorOf(filterDense).at(ids[selected]), 1.0f) << "column=" << selected;
const std::map<int, float> uniform = uniformLikelihood(ids);
filterDense.computePosterior(memory_, uniform);
@@ -1156,8 +1170,8 @@ TEST_F(BayesFilterMemoryFixture, SparsePredictionIsExactOnASingleColumn)
for(size_t i = 0; i < ids.size(); ++i)
{
EXPECT_FLOAT_EQ(filterDense.getPosterior().at(ids[i]),
filterSparse.getPosterior().at(ids[i]))
EXPECT_FLOAT_EQ(posteriorOf(filterDense).at(ids[i]),
posteriorOf(filterSparse).at(ids[i]))
<< "column=" << selected << " id=" << ids[i];
}
}
@@ -1194,11 +1208,11 @@ TEST_F(BayesFilterMemoryFixture, CompareSparsePredictionModesInLocalizationMode)
filterDense.computePosterior(memory_, likelihood);
filterSparse.computePosterior(memory_, likelihood);
ASSERT_EQ(filterDense.getPosterior().size(), filterSparse.getPosterior().size());
ASSERT_EQ(posteriorOf(filterDense).size(), posteriorOf(filterSparse).size());
for(size_t i = 0; i < ids.size(); ++i)
{
EXPECT_NEAR(filterDense.getPosterior().at(ids[i]),
filterSparse.getPosterior().at(ids[i]), 1e-5f)
EXPECT_NEAR(posteriorOf(filterDense).at(ids[i]),
posteriorOf(filterSparse).at(ids[i]), 1e-5f)
<< "iter=" << iter << " id=" << ids[i];
}
}
@@ -1243,8 +1257,8 @@ TEST_F(BayesFilterMemoryFixture, SparsePredictionInLocalizationModeIsExactOnASin
for(size_t i = 0; i < ids.size(); ++i)
{
EXPECT_FLOAT_EQ(filterDense.getPosterior().at(ids[i]),
filterSparse.getPosterior().at(ids[i]))
EXPECT_FLOAT_EQ(posteriorOf(filterDense).at(ids[i]),
posteriorOf(filterSparse).at(ids[i]))
<< "column=" << selected << " id=" << ids[i];
}
}