mirror of
https://github.com/introlab/rtabmap.git
synced 2026-10-12 21:09:50 +08:00
refactoring
This commit is contained in:
@@ -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
@@ -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
@@ -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)
|
||||
{
|
||||
|
||||
@@ -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",
|
||||
|
||||
@@ -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];
|
||||
}
|
||||
}
|
||||
|
||||
Reference in New Issue
Block a user