i/rtabmap
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap.git synced 2026-10-11 12:29:50 +08:00
Code Issues Packages Projects Releases Wiki Activity

improved tests with real data

Browse Source
This commit is contained in:
matlabbe
2026-08-19 13:54:17 -07:00
parent 0577d3a82f
commit 99aa79319b
9 changed files with 78875 additions and 74 deletions
Download Patch File Download Diff File Expand all files Collapse all files
corelib/include/rtabmap/core/BayesFilter.h
+2 -1
View File
@@ -266,7 +266,8 @@ private:
float _totalPredictionLCValues; ///< Sum of all values in _predictionLC. float _totalPredictionLCValues; ///< Sum of all values in _predictionLC.
float _predictionEpsilon; ///< Minimum non-zero probability in the model. float _predictionEpsilon; ///< Minimum non-zero probability in the model.
bool _sparsePrediction; ///< Multiply the prediction sparsely (Bayes/SparsePrediction). bool _sparsePrediction; ///< Multiply the prediction sparsely (Bayes/SparsePrediction).
bool _predictionChanged; ///< True when _prediction was rebuilt, so the sparse view is stale. bool _predictionChanged; ///< True when _prediction was rebuilt, so the sparse form is stale.
bool _sparsePredictionRejected; ///< True when the current prediction was measured as too dense to keep sparse.
Eigen::SparseMatrix<float, Eigen::RowMajor> _sparsePredictionMatrix; ///< The prediction, sparse. Built instead of _prediction over a fixed graph. Eigen::SparseMatrix<float, Eigen::RowMajor> _sparsePredictionMatrix; ///< The prediction, sparse. Built instead of _prediction over a fixed graph.
std::map<int, std::map<int, int> > _neighborsIndex; ///< Cached neighbor margins per signature id. std::map<int, std::map<int, int> > _neighborsIndex; ///< Cached neighbor margins per signature id.
}; };
corelib/src/BayesFilter.cpp
+93 -8
View File
@@ -52,7 +52,8 @@ BayesFilter::BayesFilter(const ParametersMap & parameters) :
_totalPredictionLCValues(0.0f), _totalPredictionLCValues(0.0f),
_predictionEpsilon(0.0f), _predictionEpsilon(0.0f),
_sparsePrediction(Parameters::defaultBayesSparsePrediction()), _sparsePrediction(Parameters::defaultBayesSparsePrediction()),
_predictionChanged(true) _predictionChanged(true),
_sparsePredictionRejected(false)
{ {
this->setPredictionLC(Parameters::defaultBayesPredictionLC()); this->setPredictionLC(Parameters::defaultBayesPredictionLC());
this->parseParameters(parameters); this->parseParameters(parameters);
@@ -75,6 +76,7 @@ void BayesFilter::parseParameters(const ParametersMap & parameters)
// The sparse view is rebuilt on the next posterior if it was just enabled, and // The sparse view is rebuilt on the next posterior if it was just enabled, and
// released if it was just disabled. // released if it was just disabled.
_predictionChanged = true; _predictionChanged = true;
_sparsePredictionRejected = false;
if(!_sparsePrediction) if(!_sparsePrediction)
{ {
this->clearSparsePrediction(); this->clearSparsePrediction();
@@ -133,6 +135,7 @@ void BayesFilter::setPredictionLC(const std::string & prediction)
} }
// A new model changes the values and the sparsity of the prediction matrix. // A new model changes the values and the sparsity of the prediction matrix.
_predictionChanged = true; _predictionChanged = true;
_sparsePredictionRejected = false;
} }
const std::vector<double> & BayesFilter::getPredictionLC() const const std::vector<double> & BayesFilter::getPredictionLC() const
@@ -181,6 +184,7 @@ void BayesFilter::reset()
_prediction = cv::Mat(); _prediction = cv::Mat();
this->clearSparsePrediction(); this->clearSparsePrediction();
_predictionChanged = true; _predictionChanged = true;
_sparsePredictionRejected = false;
_neighborsIndex.clear(); _neighborsIndex.clear();
} }
@@ -219,6 +223,9 @@ const std::map<int, float> & BayesFilter::computePosterior(const Memory * memory
// indexed by, taken once: there are as many of them as there are locations in the // indexed by, taken once: there are as many of them as there are locations in the
// working memory, and walking the map to collect them is not free at that size. // working memory, and walking the map to collect them is not free at that size.
const std::vector<int> ids = uKeys(likelihood); const std::vector<int> ids = uKeys(likelihood);
// Whether they are the locations of the last iteration, which over a fixed graph they
// always are: the prediction and the posterior are then both kept as they are.
const bool sameIds = this->posteriorHasSameIds(ids);
// Recursive Bayes estimation... // Recursive Bayes estimation...
// STEP 1 - Prediction : Prior*lastPosterior // STEP 1 - Prediction : Prior*lastPosterior
@@ -227,21 +234,32 @@ const std::map<int, float> & BayesFilter::computePosterior(const Memory * memory
// allocated, when the graph is fixed: in localization mode there is no incremental // allocated, when the graph is fixed: in localization mode there is no incremental
// update of the matrix to carry columns over, so nothing else needs it. While // update of the matrix to carry columns over, so nothing else needs it. While
// mapping, the matrix is built as before and the sparse form is taken from it. // mapping, the matrix is built as before and the sparse form is taken from it.
if(!sameIds)
{
// The locations changed, so the prediction has to be built again, and whether it
// is worth keeping sparse is a question about the new one.
_predictionChanged = true;
_sparsePredictionRejected = false;
}
const bool buildSparseDirectly = const bool buildSparseDirectly =
_sparsePrediction && _sparsePrediction &&
!_sparsePredictionRejected &&
_totalPredictionLCValues >= 1 && _totalPredictionLCValues >= 1 &&
!memory->isIncremental(); !memory->isIncremental();
bool sparseBuilt = false; bool sparseBuilt = false;
if(buildSparseDirectly) if(buildSparseDirectly)
{ {
if(_predictionChanged || _sparsePredictionMatrix.rows() != (int)ids.size() || if(_predictionChanged)
!this->posteriorHasSameIds(ids))
{ {
sparseBuilt = this->generateSparsePrediction(memory, ids); sparseBuilt = this->generateSparsePrediction(memory, ids);
// Measured as too dense to be worth it: the matrix is built instead, and not
// measured again until the locations or the model change. Retrying on every
// iteration would cost more than the multiplication it is trying to save.
_sparsePredictionRejected = !sparseBuilt;
} }
else else
{ {
sparseBuilt = true; sparseBuilt = _sparsePredictionMatrix.rows() > 0;
} }
UDEBUG("STEP1-generate prior=%fs, rows=%d, cols=%d", timer.ticks(), UDEBUG("STEP1-generate prior=%fs, rows=%d, cols=%d", timer.ticks(),
(int)_sparsePredictionMatrix.rows(), (int)_sparsePredictionMatrix.cols()); (int)_sparsePredictionMatrix.rows(), (int)_sparsePredictionMatrix.cols());
@@ -252,12 +270,30 @@ const std::map<int, float> & BayesFilter::computePosterior(const Memory * memory
UDEBUG("STEP1-generate prior=%fs, rows=%d, cols=%d", timer.ticks(), _prediction.rows, _prediction.cols); UDEBUG("STEP1-generate prior=%fs, rows=%d, cols=%d", timer.ticks(), _prediction.rows, _prediction.cols);
//std::cout << "Prediction=" << _prediction << std::endl; //std::cout << "Prediction=" << _prediction << std::endl;
if(_sparsePrediction && _predictionChanged) // Taking the sparse form from the matrix reads all of the matrix, which is the
// work of one dense multiplication: it pays for itself on the second
// multiplication of a prediction, not on the first. So it is left until the
// prediction is seen to outlast an iteration. A mapping session changes it on
// every location it adds and never pays for a form it would not multiply twice,
// while a session sitting on the same graph pays once and gains on every
// iteration after.
if(_sparsePrediction && !_sparsePredictionRejected && !_prediction.empty())
{ {
// Only when the matrix has changed: over a fixed graph this happens once. if(_predictionChanged)
this->updateSparsePredictionFromDense(); {
UDEBUG("STEP1-sparse prediction update time=%fs", timer.ticks()); // A prediction of its own: whatever was built for the previous one is stale.
this->clearSparsePrediction();
}
else if(_sparsePredictionMatrix.rows() != _prediction.rows)
{
// The prediction of the last iteration, so it is being multiplied more than
// once and its sparse form is worth the read.
this->updateSparsePredictionFromDense();
_sparsePredictionRejected = _sparsePredictionMatrix.rows() == 0;
UDEBUG("STEP1-sparse prediction update time=%fs", timer.ticks());
}
} }
_predictionChanged = false;
} }
// Adjust the last posterior if some images were // Adjust the last posterior if some images were
@@ -551,6 +587,55 @@ bool BayesFilter::generateSparsePrediction(const Memory * memory, const std::vec
// A value costs 12 bytes as a triplet and 8 in the matrix, against the 4 of the // A value costs 12 bytes as a triplet and 8 in the matrix, against the 4 of the
// dense one, so past a quarter filled the sparse form is not worth building. // dense one, so past a quarter filled the sparse form is not worth building.
const size_t maxValues = (size_t)size*(size_t)size/4; const size_t maxValues = (size_t)size*(size_t)size/4;
// The neighborhood of a few locations, to know whether the prediction is worth
// keeping sparse before building all of it. A loop closure link costs no margin, so
// on a densely linked graph a column reaches most of the map and there is nothing
// sparse to keep; finding that out by building a quarter of it first would cost more
// than the multiplications it is trying to save.
{
const int samples = size < 64 ? size : 64;
size_t reached = 0;
int sampled = 0;
for(int s=0; s<samples; ++s)
{
const int i = (int)((double)s*(double)size/(double)samples);
if(ids[i] <= 0)
{
continue;
}
std::list<int> idsLoopMargin;
const std::map<int, int> neighbors = resolveNeighbors(
memory, ids[i], _predictionLC.size()-1, idToIndexMap, idsLoopMargin, 0);
for(std::map<int, int>::const_iterator iter=neighbors.begin(); iter!=neighbors.end(); ++iter)
{
if(idToIndexMap.find(iter->first) != idToIndexMap.end())
{
++reached;
}
}
++sampled;
}
if(sampled > 0)
{
const double perColumn = double(reached)/double(sampled);
UDEBUG("Sparse prediction: %.0f values per column over %d locations, estimated "
"from %d of them", perColumn, size, sampled);
if(perColumn*(double)size > (double)maxValues)
{
UWARN("A column of the prediction holds %.0f of the %d locations, estimated "
"from %d of them, which is too dense for %s to be worth it: a value "
"costs 8 bytes kept sparse against the 4 of the matrix. Building the "
"matrix instead. Every loop closure link widens a column, as one "
"costs no depth in the graph search, and so does a longer %s.",
perColumn, size, sampled,
Parameters::kBayesSparsePrediction().c_str(),
Parameters::kBayesPredictionLC().c_str());
return false;
}
}
}
std::vector<float> column(size, 0.0f); std::vector<float> column(size, 0.0f);
std::vector<Eigen::Triplet<float> > triplets; std::vector<Eigen::Triplet<float> > triplets;
corelib/src/optimizer/OptimizerG2O.cpp
+93 -36
View File
@@ -2242,6 +2242,27 @@ bool OptimizerG2O::loadGraph(
std::vector<VertexEntry> verticesList; std::vector<VertexEntry> verticesList;
std::vector<EdgeEntry> edgesList; std::vector<EdgeEntry> edgesList;
// The type of a link, which saveGraph() appends as a column past the fields the
// format defines: g2o's own loader reads the fields it knows and ignores what
// follows, so the column travels with the file without breaking it. A file written
// by anything else has no such column, and the type stays the one its tag implies.
// This is the only place the type of an edge can come from: the format has no field
// for it, so a loop closure and an odometry link are otherwise the same EDGE_SE2.
const auto readType = [](const std::vector<std::string> & v, size_t definedSize, Link::Type fallback)
{
if(v.size() > definedSize)
{
const int type = atoi(v[definedSize].c_str());
if(type >= 0 && type < Link::kEnd)
{
return (Link::Type)type;
}
UWARN("Ignoring link type \"%s\", not one of the %d types.",
v[definedSize].c_str(), (int)Link::kEnd);
}
return fallback;
};
char line[2048]; char line[2048];
while(fgets(line, 2048, file) != NULL) while(fgets(line, 2048, file) != NULL)
{ {
@@ -2301,7 +2322,7 @@ bool OptimizerG2O::loadGraph(
e.definitelyLandmark = true; e.definitelyLandmark = true;
verticesList.push_back(e); verticesList.push_back(e);
} }
else if(tag == "EDGE_SE2" && v.size() == 12) else if(tag == "EDGE_SE2" && v.size() >= 12)
{ {
EdgeEntry e; EdgeEntry e;
e.from = atoi(v[1].c_str()); e.from = atoi(v[1].c_str());
@@ -2314,12 +2335,13 @@ bool OptimizerG2O::loadGraph(
e.info.at<double>(1, 1) = uStr2Double(v[9]); e.info.at<double>(1, 1) = uStr2Double(v[9]);
e.info.at<double>(1, 5) = e.info.at<double>(5, 1) = uStr2Double(v[10]); e.info.at<double>(1, 5) = e.info.at<double>(5, 1) = uStr2Double(v[10]);
e.info.at<double>(5, 5) = uStr2Double(v[11]); e.info.at<double>(5, 5) = uStr2Double(v[11]);
e.type = Link::kUndef; // disambiguated after we know landmarkOffset // kUndef is disambiguated after we know landmarkOffset
e.type = readType(v, 12, Link::kUndef);
e.isPrior = false; e.isPrior = false;
e.hasLandmarkEndpoint = false; e.hasLandmarkEndpoint = false;
edgesList.push_back(e); edgesList.push_back(e);
} }
else if(tag == "EDGE_SE2_XY" && v.size() == 8) else if(tag == "EDGE_SE2_XY" && v.size() >= 8)
{ {
EdgeEntry e; EdgeEntry e;
e.from = atoi(v[1].c_str()); e.from = atoi(v[1].c_str());
@@ -2329,12 +2351,12 @@ bool OptimizerG2O::loadGraph(
e.info.at<double>(0, 0) = uStr2Double(v[5]); e.info.at<double>(0, 0) = uStr2Double(v[5]);
e.info.at<double>(0, 1) = e.info.at<double>(1, 0) = uStr2Double(v[6]); e.info.at<double>(0, 1) = e.info.at<double>(1, 0) = uStr2Double(v[6]);
e.info.at<double>(1, 1) = uStr2Double(v[7]); e.info.at<double>(1, 1) = uStr2Double(v[7]);
e.type = Link::kLandmark; e.type = readType(v, 8, Link::kLandmark);
e.isPrior = false; e.isPrior = false;
e.hasLandmarkEndpoint = true; e.hasLandmarkEndpoint = true;
edgesList.push_back(e); edgesList.push_back(e);
} }
else if((tag == "EDGE_SE3:QUAT" || tag == "EDGE_SE3") && v.size() == 31) else if((tag == "EDGE_SE3:QUAT" || tag == "EDGE_SE3") && v.size() >= 31)
{ {
EdgeEntry e; EdgeEntry e;
e.from = atoi(v[1].c_str()); e.from = atoi(v[1].c_str());
@@ -2353,12 +2375,12 @@ bool OptimizerG2O::loadGraph(
} }
// EDGE_SE3 (no :QUAT) is the landmark variant emitted by saveGraph // EDGE_SE3 (no :QUAT) is the landmark variant emitted by saveGraph
bool landmarkTag = (tag == "EDGE_SE3"); bool landmarkTag = (tag == "EDGE_SE3");
e.type = landmarkTag ? Link::kLandmark : Link::kUndef; e.type = readType(v, 31, landmarkTag ? Link::kLandmark : Link::kUndef);
e.isPrior = false; e.isPrior = false;
e.hasLandmarkEndpoint = landmarkTag; e.hasLandmarkEndpoint = landmarkTag;
edgesList.push_back(e); edgesList.push_back(e);
} }
else if(tag == "EDGE_SE3_TRACKXYZ" && v.size() == 13) else if(tag == "EDGE_SE3_TRACKXYZ" && v.size() >= 13)
{ {
EdgeEntry e; EdgeEntry e;
e.from = atoi(v[1].c_str()); e.from = atoi(v[1].c_str());
@@ -2372,12 +2394,12 @@ bool OptimizerG2O::loadGraph(
e.info.at<double>(1, 1) = uStr2Double(v[10]); e.info.at<double>(1, 1) = uStr2Double(v[10]);
e.info.at<double>(1, 2) = e.info.at<double>(2, 1) = uStr2Double(v[11]); e.info.at<double>(1, 2) = e.info.at<double>(2, 1) = uStr2Double(v[11]);
e.info.at<double>(2, 2) = uStr2Double(v[12]); e.info.at<double>(2, 2) = uStr2Double(v[12]);
e.type = Link::kLandmark; e.type = readType(v, 13, Link::kLandmark);
e.isPrior = false; e.isPrior = false;
e.hasLandmarkEndpoint = true; e.hasLandmarkEndpoint = true;
edgesList.push_back(e); edgesList.push_back(e);
} }
else if(tag == "EDGE_PRIOR_SE2" && v.size() == 11) else if(tag == "EDGE_PRIOR_SE2" && v.size() >= 11)
{ {
EdgeEntry e; EdgeEntry e;
e.from = atoi(v[1].c_str()); e.from = atoi(v[1].c_str());
@@ -2390,12 +2412,12 @@ bool OptimizerG2O::loadGraph(
e.info.at<double>(1, 1) = uStr2Double(v[8]); e.info.at<double>(1, 1) = uStr2Double(v[8]);
e.info.at<double>(1, 5) = e.info.at<double>(5, 1) = uStr2Double(v[9]); e.info.at<double>(1, 5) = e.info.at<double>(5, 1) = uStr2Double(v[9]);
e.info.at<double>(5, 5) = uStr2Double(v[10]); e.info.at<double>(5, 5) = uStr2Double(v[10]);
e.type = Link::kPosePrior; e.type = readType(v, 11, Link::kPosePrior);
e.isPrior = true; e.isPrior = true;
e.hasLandmarkEndpoint = false; e.hasLandmarkEndpoint = false;
edgesList.push_back(e); edgesList.push_back(e);
} }
else if(tag == "EDGE_PRIOR_SE2_XY" && v.size() == 7) else if(tag == "EDGE_PRIOR_SE2_XY" && v.size() >= 7)
{ {
EdgeEntry e; EdgeEntry e;
e.from = atoi(v[1].c_str()); e.from = atoi(v[1].c_str());
@@ -2407,12 +2429,12 @@ bool OptimizerG2O::loadGraph(
e.info.at<double>(1, 1) = uStr2Double(v[6]); e.info.at<double>(1, 1) = uStr2Double(v[6]);
// no orientation info on this prior // no orientation info on this prior
e.info.at<double>(3, 3) = e.info.at<double>(4, 4) = e.info.at<double>(5, 5) = 1.0 / 9999.0; e.info.at<double>(3, 3) = e.info.at<double>(4, 4) = e.info.at<double>(5, 5) = 1.0 / 9999.0;
e.type = Link::kPosePrior; e.type = readType(v, 7, Link::kPosePrior);
e.isPrior = true; e.isPrior = true;
e.hasLandmarkEndpoint = false; e.hasLandmarkEndpoint = false;
edgesList.push_back(e); edgesList.push_back(e);
} }
else if(tag == "EDGE_SE3_PRIOR" && v.size() == 31) else if(tag == "EDGE_SE3_PRIOR" && v.size() >= 31)
{ {
EdgeEntry e; EdgeEntry e;
e.from = atoi(v[1].c_str()); e.from = atoi(v[1].c_str());
@@ -2430,12 +2452,12 @@ bool OptimizerG2O::loadGraph(
if(r != c) e.info.at<double>(c, r) = e.info.at<double>(r, c); if(r != c) e.info.at<double>(c, r) = e.info.at<double>(r, c);
} }
} }
e.type = Link::kPosePrior; e.type = readType(v, 31, Link::kPosePrior);
e.isPrior = true; e.isPrior = true;
e.hasLandmarkEndpoint = false; e.hasLandmarkEndpoint = false;
edgesList.push_back(e); edgesList.push_back(e);
} }
else if(tag == "EDGE_POINTXYZ_PRIOR" && v.size() == 11) else if(tag == "EDGE_POINTXYZ_PRIOR" && v.size() >= 11)
{ {
EdgeEntry e; EdgeEntry e;
e.from = atoi(v[1].c_str()); e.from = atoi(v[1].c_str());
@@ -2450,12 +2472,12 @@ bool OptimizerG2O::loadGraph(
e.info.at<double>(2, 2) = uStr2Double(v[10]); e.info.at<double>(2, 2) = uStr2Double(v[10]);
// no orientation info on this prior // no orientation info on this prior
e.info.at<double>(3, 3) = e.info.at<double>(4, 4) = e.info.at<double>(5, 5) = 1.0 / 9999.0; e.info.at<double>(3, 3) = e.info.at<double>(4, 4) = e.info.at<double>(5, 5) = 1.0 / 9999.0;
e.type = Link::kPosePrior; e.type = readType(v, 11, Link::kPosePrior);
e.isPrior = true; e.isPrior = true;
e.hasLandmarkEndpoint = false; e.hasLandmarkEndpoint = false;
edgesList.push_back(e); edgesList.push_back(e);
} }
else if(tag == "EDGE_SE2_SWITCHABLE" && v.size() == 13) else if(tag == "EDGE_SE2_SWITCHABLE" && v.size() >= 13)
{ {
EdgeEntry e; EdgeEntry e;
e.from = atoi(v[1].c_str()); e.from = atoi(v[1].c_str());
@@ -2469,12 +2491,12 @@ bool OptimizerG2O::loadGraph(
e.info.at<double>(1, 1) = uStr2Double(v[10]); e.info.at<double>(1, 1) = uStr2Double(v[10]);
e.info.at<double>(1, 5) = e.info.at<double>(5, 1) = uStr2Double(v[11]); e.info.at<double>(1, 5) = e.info.at<double>(5, 1) = uStr2Double(v[11]);
e.info.at<double>(5, 5) = uStr2Double(v[12]); e.info.at<double>(5, 5) = uStr2Double(v[12]);
e.type = Link::kUndef; e.type = readType(v, 13, Link::kUndef);
e.isPrior = false; e.isPrior = false;
e.hasLandmarkEndpoint = false; e.hasLandmarkEndpoint = false;
edgesList.push_back(e); edgesList.push_back(e);
} }
else if(tag == "EDGE_SE3_SWITCHABLE" && v.size() == 32) else if(tag == "EDGE_SE3_SWITCHABLE" && v.size() >= 32)
{ {
EdgeEntry e; EdgeEntry e;
e.from = atoi(v[1].c_str()); e.from = atoi(v[1].c_str());
@@ -2492,7 +2514,7 @@ bool OptimizerG2O::loadGraph(
if(r != c) e.info.at<double>(c, r) = e.info.at<double>(r, c); if(r != c) e.info.at<double>(c, r) = e.info.at<double>(r, c);
} }
} }
e.type = Link::kUndef; e.type = readType(v, 32, Link::kUndef);
e.isPrior = false; e.isPrior = false;
e.hasLandmarkEndpoint = false; e.hasLandmarkEndpoint = false;
edgesList.push_back(e); edgesList.push_back(e);
@@ -2743,8 +2765,35 @@ bool OptimizerG2O::saveGraph(
} }
int virtualVertexId = landmarkOffset - (poses.size()&&poses.rbegin()->first<0?poses.rbegin()->first:0); int virtualVertexId = landmarkOffset - (poses.size()&&poses.rbegin()->first<0?poses.rbegin()->first:0);
// A link is stored on both of the nodes it connects, so a caller iterating them
// hands us each one twice, once per direction. g2o has no notion of a reverse
// edge: it would read the two lines as two independent constraints and count the
// information of every link twice. Only the first direction of a pair is written,
// which is also half the file. Links on a single node (a prior, gravity) are not
// pairs and are left alone.
std::set<std::pair<int, int> > writtenPairs;
for(std::multimap<int, Link>::const_iterator iter = edgeConstraints.begin(); iter!=edgeConstraints.end(); ++iter) for(std::multimap<int, Link>::const_iterator iter = edgeConstraints.begin(); iter!=edgeConstraints.end(); ++iter)
{ {
if(iter->second.from() != iter->second.to())
{
const std::pair<int, int> pair(
std::min(iter->second.from(), iter->second.to()),
std::max(iter->second.from(), iter->second.to()));
if(!writtenPairs.insert(pair).second)
{
continue;
}
}
// The type of the link, as a column past the fields the format defines. g2o's
// own loader reads the fields it knows and ignores what follows, so this
// travels with the file without breaking it, and loadGraph() reads it back.
// Without it the type is lost on export, and the type is what tells a loop
// closure from an odometry link.
const std::string typeSuffix = uFormat(" %d", (int)iter->second.type());
if (iter->second.type() == Link::kLandmark) if (iter->second.type() == Link::kLandmark)
{ {
if (this->landmarksIgnored()) if (this->landmarksIgnored())
@@ -2760,7 +2809,7 @@ bool OptimizerG2O::saveGraph(
if(uValue(isLandmarkWithRotation, landmarkId, false)) if(uValue(isLandmarkWithRotation, landmarkId, false))
{ {
// EDGE_SE2 observed_vertex_id observing_vertex_id x y qx qy qz qw inf_11 inf_12 inf_13 inf_22 inf_23 inf_33 // EDGE_SE2 observed_vertex_id observing_vertex_id x y qx qy qz qw inf_11 inf_12 inf_13 inf_22 inf_23 inf_33
fprintf(file, "EDGE_SE2 %d %d %f %f %f %f %f %f %f %f %f\n", fprintf(file, "EDGE_SE2 %d %d %f %f %f %f %f %f %f %f %f%s\n",
iter->second.from()<0?landmarkOffset-iter->second.from():iter->second.from(), iter->second.from()<0?landmarkOffset-iter->second.from():iter->second.from(),
iter->second.to()<0?landmarkOffset-iter->second.to():iter->second.to(), iter->second.to()<0?landmarkOffset-iter->second.to():iter->second.to(),
iter->second.transform().x(), iter->second.transform().x(),
@@ -2771,19 +2820,21 @@ bool OptimizerG2O::saveGraph(
iter->second.infMatrix().at<double>(0, 5), iter->second.infMatrix().at<double>(0, 5),
iter->second.infMatrix().at<double>(1, 1), iter->second.infMatrix().at<double>(1, 1),
iter->second.infMatrix().at<double>(1, 5), iter->second.infMatrix().at<double>(1, 5),
iter->second.infMatrix().at<double>(5, 5)); iter->second.infMatrix().at<double>(5, 5),
typeSuffix.c_str());
} }
else else
{ {
// EDGE_SE2_XY observed_vertex_id observing_vertex_id x y inf_11 inf_12 inf_22 // EDGE_SE2_XY observed_vertex_id observing_vertex_id x y inf_11 inf_12 inf_22
fprintf(file, "EDGE_SE2_XY %d %d %f %f %f %f %f\n", fprintf(file, "EDGE_SE2_XY %d %d %f %f %f %f %f%s\n",
iter->second.from()<0?landmarkOffset-iter->second.from():iter->second.from(), iter->second.from()<0?landmarkOffset-iter->second.from():iter->second.from(),
iter->second.to()<0?landmarkOffset-iter->second.to():iter->second.to(), iter->second.to()<0?landmarkOffset-iter->second.to():iter->second.to(),
iter->second.transform().x(), iter->second.transform().x(),
iter->second.transform().y(), iter->second.transform().y(),
iter->second.infMatrix().at<double>(0, 0), iter->second.infMatrix().at<double>(0, 0),
iter->second.infMatrix().at<double>(0, 1), iter->second.infMatrix().at<double>(0, 1),
iter->second.infMatrix().at<double>(1, 1)); iter->second.infMatrix().at<double>(1, 1),
typeSuffix.c_str());
} }
} }
else else
@@ -2792,7 +2843,7 @@ bool OptimizerG2O::saveGraph(
{ {
// EDGE_SE3 observed_vertex_id observing_vertex_id x y z qx qy qz qw inf_11 inf_12 .. inf_16 inf_22 .. inf_66 // EDGE_SE3 observed_vertex_id observing_vertex_id x y z qx qy qz qw inf_11 inf_12 .. inf_16 inf_22 .. inf_66
Eigen::Quaternionf q = iter->second.transform().getQuaternionf(); Eigen::Quaternionf q = iter->second.transform().getQuaternionf();
fprintf(file, "EDGE_SE3 %d %d %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f\n", fprintf(file, "EDGE_SE3 %d %d %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f%s\n",
iter->second.from()<0?landmarkOffset-iter->second.from():iter->second.from(), iter->second.from()<0?landmarkOffset-iter->second.from():iter->second.from(),
iter->second.to()<0?landmarkOffset-iter->second.to():iter->second.to(), iter->second.to()<0?landmarkOffset-iter->second.to():iter->second.to(),
iter->second.transform().x(), iter->second.transform().x(),
@@ -2822,12 +2873,13 @@ bool OptimizerG2O::saveGraph(
iter->second.infMatrix().at<double>(3, 5), iter->second.infMatrix().at<double>(3, 5),
iter->second.infMatrix().at<double>(4, 4), iter->second.infMatrix().at<double>(4, 4),
iter->second.infMatrix().at<double>(4, 5), iter->second.infMatrix().at<double>(4, 5),
iter->second.infMatrix().at<double>(5, 5)); iter->second.infMatrix().at<double>(5, 5),
typeSuffix.c_str());
} }
else else
{ {
// EDGE_SE3_TRACKXYZ observed_vertex_id observing_vertex_id param_offset x y z inf_11 inf_12 inf_13 inf_22 inf_23 inf_33 // EDGE_SE3_TRACKXYZ observed_vertex_id observing_vertex_id param_offset x y z inf_11 inf_12 inf_13 inf_22 inf_23 inf_33
fprintf(file, "EDGE_SE3_TRACKXYZ %d %d %d %f %f %f %f %f %f %f %f %f\n", fprintf(file, "EDGE_SE3_TRACKXYZ %d %d %d %f %f %f %f %f %f %f %f %f%s\n",
iter->second.from()<0?landmarkOffset-iter->second.from():iter->second.from(), iter->second.from()<0?landmarkOffset-iter->second.from():iter->second.from(),
iter->second.to()<0?landmarkOffset-iter->second.to():iter->second.to(), iter->second.to()<0?landmarkOffset-iter->second.to():iter->second.to(),
PARAM_OFFSET, PARAM_OFFSET,
@@ -2839,7 +2891,8 @@ bool OptimizerG2O::saveGraph(
iter->second.infMatrix().at<double>(0, 2), iter->second.infMatrix().at<double>(0, 2),
iter->second.infMatrix().at<double>(1, 1), iter->second.infMatrix().at<double>(1, 1),
iter->second.infMatrix().at<double>(1, 2), iter->second.infMatrix().at<double>(1, 2),
iter->second.infMatrix().at<double>(2, 2)); iter->second.infMatrix().at<double>(2, 2),
typeSuffix.c_str());
} }
} }
continue; continue;
@@ -2911,7 +2964,7 @@ bool OptimizerG2O::saveGraph(
{ {
// EDGE_SE2 observed_vertex_id observing_vertex_id x y qx qy qz qw inf_11 inf_12 inf_13 inf_22 inf_23 inf_33 // EDGE_SE2 observed_vertex_id observing_vertex_id x y qx qy qz qw inf_11 inf_12 inf_13 inf_22 inf_23 inf_33
// EDGE_SE2_PRIOR observed_vertex_id x y qx qy qz qw inf_11 inf_12 inf_13 inf_22 inf_23 inf_33 // EDGE_SE2_PRIOR observed_vertex_id x y qx qy qz qw inf_11 inf_12 inf_13 inf_22 inf_23 inf_33
fprintf(file, "%s %d%s%s %f %f %f %f %f %f %f %f %f\n", fprintf(file, "%s %d%s%s %f %f %f %f %f %f %f %f %f%s\n",
prefix.c_str(), prefix.c_str(),
iter->second.from(), iter->second.from(),
to.c_str(), to.c_str(),
@@ -2924,13 +2977,14 @@ bool OptimizerG2O::saveGraph(
iter->second.infMatrix().at<double>(0, 5), iter->second.infMatrix().at<double>(0, 5),
iter->second.infMatrix().at<double>(1, 1), iter->second.infMatrix().at<double>(1, 1),
iter->second.infMatrix().at<double>(1, 5), iter->second.infMatrix().at<double>(1, 5),
iter->second.infMatrix().at<double>(5, 5)); iter->second.infMatrix().at<double>(5, 5),
typeSuffix.c_str());
} }
else else
{ {
// EDGE_XY observed_vertex_id observing_vertex_id x y inf_11 inf_12 inf_22 // EDGE_XY observed_vertex_id observing_vertex_id x y inf_11 inf_12 inf_22
// EDGE_POINTXY_PRIOR x y inf_11 inf_12 inf_22 // EDGE_POINTXY_PRIOR x y inf_11 inf_12 inf_22
fprintf(file, "%s %d%s%s %f %f %f %f %f\n", fprintf(file, "%s %d%s%s %f %f %f %f %f%s\n",
prefix.c_str(), prefix.c_str(),
iter->second.from(), iter->second.from(),
to.c_str(), to.c_str(),
@@ -2939,7 +2993,8 @@ bool OptimizerG2O::saveGraph(
iter->second.transform().y(), iter->second.transform().y(),
iter->second.infMatrix().at<double>(0, 0), iter->second.infMatrix().at<double>(0, 0),
iter->second.infMatrix().at<double>(0, 1), iter->second.infMatrix().at<double>(0, 1),
iter->second.infMatrix().at<double>(1, 1)); iter->second.infMatrix().at<double>(1, 1),
typeSuffix.c_str());
} }
} }
else else
@@ -2949,7 +3004,7 @@ bool OptimizerG2O::saveGraph(
// EDGE_SE3 observed_vertex_id observing_vertex_id x y z qx qy qz qw inf_11 inf_12 .. inf_16 inf_22 .. inf_66 // EDGE_SE3 observed_vertex_id observing_vertex_id x y z qx qy qz qw inf_11 inf_12 .. inf_16 inf_22 .. inf_66
// EDGE_SE3_PRIOR observed_vertex_id offset_parameter_id x y z qx qy qz qw inf_11 inf_12 .. inf_16 inf_22 .. inf_66 // EDGE_SE3_PRIOR observed_vertex_id offset_parameter_id x y z qx qy qz qw inf_11 inf_12 .. inf_16 inf_22 .. inf_66
Eigen::Quaternionf q = iter->second.transform().getQuaternionf(); Eigen::Quaternionf q = iter->second.transform().getQuaternionf();
fprintf(file, "%s %d%s%s %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f\n", fprintf(file, "%s %d%s%s %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f%s\n",
prefix.c_str(), prefix.c_str(),
iter->second.from(), iter->second.from(),
to.c_str(), to.c_str(),
@@ -2981,13 +3036,14 @@ bool OptimizerG2O::saveGraph(
iter->second.infMatrix().at<double>(3, 5), iter->second.infMatrix().at<double>(3, 5),
iter->second.infMatrix().at<double>(4, 4), iter->second.infMatrix().at<double>(4, 4),
iter->second.infMatrix().at<double>(4, 5), iter->second.infMatrix().at<double>(4, 5),
iter->second.infMatrix().at<double>(5, 5)); iter->second.infMatrix().at<double>(5, 5),
typeSuffix.c_str());
} }
else else
{ {
// EDGE_XYZ observed_vertex_id observing_vertex_id x y z qx qy qz qw inf_11 inf_12 .. inf_13 inf_22 .. inf_33 // EDGE_XYZ observed_vertex_id observing_vertex_id x y z qx qy qz qw inf_11 inf_12 .. inf_13 inf_22 .. inf_33
// EDGE_POINTXYZ_PRIOR observed_vertex_id x y z inf_11 inf_12 .. inf_13 inf_22 .. inf_33 // EDGE_POINTXYZ_PRIOR observed_vertex_id x y z inf_11 inf_12 .. inf_13 inf_22 .. inf_33
fprintf(file, "%s %d%s%s %f %f %f %f %f %f %f %f %f\n", fprintf(file, "%s %d%s%s %f %f %f %f %f %f %f %f %f%s\n",
prefix.c_str(), prefix.c_str(),
iter->second.from(), iter->second.from(),
to.c_str(), to.c_str(),
@@ -3000,7 +3056,8 @@ bool OptimizerG2O::saveGraph(
iter->second.infMatrix().at<double>(0, 2), iter->second.infMatrix().at<double>(0, 2),
iter->second.infMatrix().at<double>(1, 1), iter->second.infMatrix().at<double>(1, 1),
iter->second.infMatrix().at<double>(1, 2), iter->second.infMatrix().at<double>(1, 2),
iter->second.infMatrix().at<double>(2, 2)); iter->second.infMatrix().at<double>(2, 2),
typeSuffix.c_str());
} }
} }
} }
corelib/test/CMakeLists.txt
+5 -3
View File
@@ -122,11 +122,13 @@ IF(BUILD_PERF_TESTS)
# Comparison of the dense and the sparse prediction x posterior multiplication of # Comparison of the dense and the sparse prediction x posterior multiplication of
# BayesFilter (Bayes/SparsePrediction), against the size of the map, how connected # BayesFilter (Bayes/SparsePrediction), against the size of the map, how connected
# its graph is and the depth of the prediction model: # its graph is and the depth of the prediction model. Over synthetic graphs, and over
# the graph of a real map read from data/tests/large_reduced_graph.g2o, whose link
# types decide how much of the map a column of the prediction holds:
# bin/test_bayesfilter_perf # bin/test_bayesfilter_perf
# bin/test_bayesfilter_perf --gtest_filter=-*LargeMap* # bin/test_bayesfilter_perf --gtest_filter=-*LargeMap*:-*RealMap*
# Its own executable: it spends its time benchmarking rather than asserting, and the # Its own executable: it spends its time benchmarking rather than asserting, and the
# largest map it builds allocates a gigabyte for the dense prediction matrix, # largest maps it builds allocate a gigabyte for the dense prediction matrix,
# which in a unit test shard would look like a leak. # which in a unit test shard would look like a leak.
add_executable(test_bayesfilter_perf perf_bayesfilter.cpp) add_executable(test_bayesfilter_perf perf_bayesfilter.cpp)
target_link_libraries(test_bayesfilter_perf gtest_main rtabmap_core) target_link_libraries(test_bayesfilter_perf gtest_main rtabmap_core)
corelib/test/perf_bayesfilter.cpp
+379 -20
View File
@@ -13,12 +13,15 @@
// the numbers below compare two ways of computing the same thing. // the numbers below compare two ways of computing the same thing.
#include <gtest/gtest.h> #include <gtest/gtest.h>
#include <rtabmap/core/BayesFilter.h> #include <rtabmap/core/BayesFilter.h>
#include <rtabmap/core/Graph.h>
#include <rtabmap/core/Link.h> #include <rtabmap/core/Link.h>
#include <rtabmap/core/Memory.h> #include <rtabmap/core/Memory.h>
#include <rtabmap/core/Optimizer.h>
#include <rtabmap/core/Parameters.h> #include <rtabmap/core/Parameters.h>
#include <rtabmap/core/SensorData.h> #include <rtabmap/core/SensorData.h>
#include <rtabmap/core/Signature.h> #include <rtabmap/core/Signature.h>
#include <rtabmap/core/Transform.h> #include <rtabmap/core/Transform.h>
#include <rtabmap/utilite/UFile.h>
#include <rtabmap/utilite/UTimer.h> #include <rtabmap/utilite/UTimer.h>
#include <algorithm> #include <algorithm>
#include <cmath> #include <cmath>
@@ -150,6 +153,195 @@ private:
double buildTime_ = 0.0; double buildTime_ = 0.0;
}; };
// A real map's graph, read from the g2o file it was exported to. What makes it worth
// measuring against the synthetic graphs above is its link types: the file holds mostly
// merged neighbor links, which cost a margin like an ordinary neighbor, and only a few
// hundred closures that Memory::getNeighborsId() follows without spending one. How many
// of those there are is what decides how much of the map a column of the prediction
// holds, so a real graph's answer is not a synthetic one's.
//
// The graph is rebuilt in a Memory rather than optimized: Memory::update() creates a
// signature per pose and links each to the previous one, so the links the file does not
// have are removed and the ones it has are added with their own type.
// A real map's graph, read from the g2o file it was exported to. What makes it worth
// measuring against the synthetic graphs above is its link types: how many links
// Memory::getNeighborsId() follows without spending a margin is what decides how much of
// the map a column of the prediction holds, and a real graph's answer is not a synthetic
// one's. A graph that went through the reduction holds mostly merged neighbor links,
// which cost a margin like an ordinary neighbor; one that did not holds none.
struct RealGraph
{
std::vector<int> ids; // the locations, in the order they were created
std::map<int, Transform> poses;
// The links between two locations, as indices into ids, each on the later of the two:
// that is the one a mapping session adds them on.
std::vector<std::vector<std::pair<size_t, Link::Type> > > linksTo;
std::map<int, int> byType;
int skippedLinks = 0;
double loadTime = 0.0;
};
bool loadRealGraph(const std::string & path, RealGraph & graph)
{
UTimer timer;
std::map<int, Transform> poses;
std::multimap<int, Link> links;
if(!graph::importPoses(path, 4 /*g2o*/, poses, &links))
{
return false;
}
graph.loadTime = timer.ticks();
graph.poses = poses;
for(std::map<int, Transform>::const_iterator iter=poses.begin(); iter!=poses.end(); ++iter)
{
if(iter->first > 0)
{
graph.ids.push_back(iter->first);
}
}
std::sort(graph.ids.begin(), graph.ids.end());
std::map<int, size_t> indexOf;
for(size_t i=0; i<graph.ids.size(); ++i)
{
indexOf.insert(std::make_pair(graph.ids[i], i));
}
// One entry per pair of locations. Links on a single location (a prior, gravity) and
// landmark observations are left out: getNeighborsId() doesn't walk the first, and the
// second would need the landmark index of a memory that mapped them.
graph.linksTo.resize(graph.ids.size());
std::set<std::pair<size_t, size_t> > seen;
for(std::multimap<int, Link>::const_iterator iter=links.begin(); iter!=links.end(); ++iter)
{
const Link & link = iter->second;
if(link.from() == link.to() || link.from() < 0 || link.to() < 0)
{
++graph.skippedLinks;
continue;
}
const size_t a = indexOf.at(link.from()), b = indexOf.at(link.to());
if(!seen.insert(std::make_pair(std::min(a,b), std::max(a,b))).second)
{
continue;
}
graph.linksTo[std::max(a,b)].push_back(std::make_pair(std::min(a,b), link.type()));
++graph.byType[link.type()];
}
return true;
}
// Adds one location and the links the graph has between it and the ones already there.
// Memory::update() links each new signature to the previous one as a kNeighbor, so that
// one is dropped when the graph does not have it, or has it with another type.
void addRealNode(
Memory * memory,
const RealGraph & graph,
size_t index,
std::vector<int> & newIds,
int * removedLinks = 0)
{
static const cv::Mat image(8, 8, CV_8UC1, cv::Scalar(128));
static const cv::Mat covariance = cv::Mat::eye(6, 6, CV_64FC1) * 0.0001;
static const cv::Mat information = cv::Mat::eye(6, 6, CV_64FC1);
SensorData data(image);
UASSERT(memory->update(data, graph.poses.at(graph.ids[index]), covariance));
newIds.push_back(memory->getLastSignatureId());
bool previousLinked = false;
for(size_t i=0; i<graph.linksTo[index].size(); ++i)
{
const size_t other = graph.linksTo[index][i].first;
const Link::Type type = graph.linksTo[index][i].second;
if(other == index-1 && type == Link::kNeighbor)
{
previousLinked = true; // update() already made this one
continue;
}
memory->addLink(Link(newIds[index], newIds[other], type,
Transform::getIdentity(), information));
}
if(index > 0 && !previousLinked)
{
// Either the graph has no link between these two, or it has one of another type
// which the loop above has just added.
memory->removeLink(newIds[index-1], newIds[index]);
if(removedLinks)
{
++(*removedLinks);
}
}
}
Memory * newRealMemory()
{
ParametersMap params;
params.insert(ParametersPair(Parameters::kKpMaxFeatures(), "-1"));
params.insert(ParametersPair(Parameters::kMemSTMSize(), "1"));
params.insert(ParametersPair(Parameters::kMemRehearsalSimilarity(), "1.0"));
params.insert(ParametersPair(Parameters::kMemBinDataKept(), "false"));
return new Memory(params);
}
// What Rtabmap passes to the filter: the virtual place followed by the locations of the
// working memory that are not in the short term memory.
std::vector<int> bayesIdsOf(const Memory * memory)
{
std::vector<int> ids;
ids.push_back(Memory::kIdVirtual);
const std::set<int> & stm = memory->getStMem();
for(std::map<int, double>::const_iterator iter=memory->getWorkingMem().begin();
iter!=memory->getWorkingMem().end();
++iter)
{
if(iter->first > 0 && stm.find(iter->first) == stm.end())
{
ids.push_back(iter->first);
}
}
return ids;
}
std::map<int, float> uniformLikelihoodOf(const std::vector<int> & ids)
{
std::map<int, float> likelihood;
for(size_t i=0; i<ids.size(); ++i)
{
likelihood.insert(std::make_pair(ids[i], 1.0f));
}
return likelihood;
}
const char * linkTypeName(int type)
{
switch(type)
{
case Link::kNeighbor: return "kNeighbor";
case Link::kGlobalClosure: return "kGlobalClosure";
case Link::kLocalSpaceClosure: return "kLocalSpaceClosure";
case Link::kLocalTimeClosure: return "kLocalTimeClosure";
case Link::kUserClosure: return "kUserClosure";
case Link::kVirtualClosure: return "kVirtualClosure";
case Link::kNeighborMerged: return "kNeighborMerged";
case Link::kPosePrior: return "kPosePrior";
case Link::kLandmark: return "kLandmark";
case Link::kGravity: return "kGravity";
default: return "other";
}
}
void printGraph(const std::string & path, const RealGraph & graph, int removedLinks)
{
std::cout << "[ ] " << path << ": read in " << graph.loadTime << "s, "
<< graph.ids.size() << " locations, links:";
for(std::map<int,int>::const_iterator iter=graph.byType.begin(); iter!=graph.byType.end(); ++iter)
{
std::cout << " " << linkTypeName(iter->first) << "=" << iter->second;
}
std::cout << " (" << graph.skippedLinks << " on a single location or on a landmark not rebuilt, "
<< removedLinks << " gaps in the chain)" << std::endl;
}
struct Result struct Result
{ {
double firstIteration = 0.0; // includes generating the prediction, sparse or dense double firstIteration = 0.0; // includes generating the prediction, sparse or dense
@@ -158,19 +350,21 @@ struct Result
std::map<int, float> posterior; std::map<int, float> posterior;
}; };
Result run(const SyntheticMap & map, const char * predictionLC, bool sparse, int iterations) Result run(const Memory * memory,
const std::vector<int> & ids,
const std::map<int, float> & likelihood,
const char * predictionLC,
bool sparse,
int iterations)
{ {
ParametersMap params; ParametersMap params;
params.insert(ParametersPair(Parameters::kBayesPredictionLC(), predictionLC)); params.insert(ParametersPair(Parameters::kBayesPredictionLC(), predictionLC));
params.insert(ParametersPair(Parameters::kBayesSparsePrediction(), sparse?"true":"false")); params.insert(ParametersPair(Parameters::kBayesSparsePrediction(), sparse?"true":"false"));
BayesFilter filter(params); BayesFilter filter(params);
const std::vector<int> ids = map.bayesIds();
const std::map<int, float> likelihood = map.uniformLikelihood(ids);
Result result; Result result;
UTimer timer; UTimer timer;
filter.computePosterior(map.memory(), likelihood); filter.computePosterior(memory, likelihood);
result.firstIteration = timer.ticks(); result.firstIteration = timer.ticks();
// A few untimed iterations to let the caches and the processor clock settle before // A few untimed iterations to let the caches and the processor clock settle before
@@ -180,7 +374,7 @@ Result run(const SyntheticMap & map, const char * predictionLC, bool sparse, int
// the slow one and leave the two posteriors nowhere near each other. // the slow one and leave the two posteriors nowhere near each other.
for(int i=0; i<3; ++i) for(int i=0; i<3; ++i)
{ {
filter.computePosterior(map.memory(), likelihood); filter.computePosterior(memory, likelihood);
} }
// The fastest iteration rather than the mean or the median of them. Everything that // The fastest iteration rather than the mean or the median of them. Everything that
@@ -193,7 +387,7 @@ Result run(const SyntheticMap & map, const char * predictionLC, bool sparse, int
for(int i=1; i<iterations; ++i) for(int i=1; i<iterations; ++i)
{ {
timer.restart(); timer.restart();
filter.computePosterior(map.memory(), likelihood); filter.computePosterior(memory, likelihood);
const double elapsed = timer.ticks(); const double elapsed = timer.ticks();
if(best == 0.0 || elapsed < best) if(best == 0.0 || elapsed < best)
{ {
@@ -237,11 +431,30 @@ void report(const char * name, const Result & result)
name, result.firstIteration*1000.0, result.steadyState*1000.0, result.memoryUsed/1048576.0); name, result.firstIteration*1000.0, result.steadyState*1000.0, result.memoryUsed/1048576.0);
} }
void compare(const SyntheticMap & map, const char * predictionLC, int iterations) // sparseFirst measures the sparse mode before the dense one. It matters on a large map:
// the dense mode allocates the prediction matrix, and running it first leaves the
// allocator holding hundreds of megabytes, which the sparse measurement that follows then
// pays for. Measuring the two in separate processes is the only way to have both clean;
// within one, the cheaper mode is the one to protect.
void compare(const Memory * memory,
const std::vector<int> & ids,
const std::map<int, float> & likelihood,
const char * predictionLC,
int iterations,
bool sparseFirst = false)
{ {
const size_t size = map.bayesIds().size(); const size_t size = ids.size();
const Result dense = run(map, predictionLC, false, iterations); Result dense, sparse;
const Result sparse = run(map, predictionLC, true, iterations); if(sparseFirst)
{
sparse = run(memory, ids, likelihood, predictionLC, true, iterations);
dense = run(memory, ids, likelihood, predictionLC, false, iterations);
}
else
{
dense = run(memory, ids, likelihood, predictionLC, false, iterations);
sparse = run(memory, ids, likelihood, predictionLC, true, iterations);
}
report("dense", dense); report("dense", dense);
report("sparse", sparse); report("sparse", sparse);
@@ -285,7 +498,8 @@ TEST(BayesFilterPerfTest, DenseVsSparsePredictionOnGrowingMaps)
<< map.loopClosures() << " loop closures, graph built in " << map.buildTime() << map.loopClosures() << " loop closures, graph built in " << map.buildTime()
<< "s, dense matrix = " << (size*size*sizeof(float))/1048576 << " MB" << std::endl; << "s, dense matrix = " << (size*size*sizeof(float))/1048576 << " MB" << std::endl;
compare(map, PREDICTION_DEFAULT, iterations); const std::vector<int> ids = map.bayesIds();
compare(map.memory(), ids, map.uniformLikelihood(ids), PREDICTION_DEFAULT, iterations);
} }
} }
@@ -313,10 +527,12 @@ TEST(BayesFilterPerfTest, SparsityAgainstGraphConnectivityAndModelDepth)
std::cout << "[ ] " << nodes << " nodes, " << connectivities[c].name std::cout << "[ ] " << nodes << " nodes, " << connectivities[c].name
<< " (" << map.loopClosures() << " loop closures)" << std::endl; << " (" << map.loopClosures() << " loop closures)" << std::endl;
const std::vector<int> ids = map.bayesIds();
const std::map<int, float> likelihood = map.uniformLikelihood(ids);
std::cout << "[ ] 18 values model (default, depth 17):" << std::endl; std::cout << "[ ] 18 values model (default, depth 17):" << std::endl;
compare(map, PREDICTION_DEFAULT, iterations); compare(map.memory(), ids, likelihood, PREDICTION_DEFAULT, iterations);
std::cout << "[ ] 8 values model (depth 7, every value above 1e-4):" << std::endl; std::cout << "[ ] 8 values model (depth 7, every value above 1e-4):" << std::endl;
compare(map, PREDICTION_TRUNCATED, iterations); compare(map.memory(), ids, likelihood, PREDICTION_TRUNCATED, iterations);
} }
} }
@@ -339,15 +555,20 @@ TEST(BayesFilterPerfTest, DenseVsSparsePredictionOnALargeMap)
<< map.loopClosures() << " loop closures, graph built in " << map.buildTime() << map.loopClosures() << " loop closures, graph built in " << map.buildTime()
<< "s, dense matrix = " << (size*size*sizeof(float))/1048576 << " MB" << std::endl; << "s, dense matrix = " << (size*size*sizeof(float))/1048576 << " MB" << std::endl;
const std::vector<int> ids = map.bayesIds();
const std::map<int, float> likelihood = map.uniformLikelihood(ids);
std::cout << "[ ] 18 values model (default, depth 17):" << std::endl; std::cout << "[ ] 18 values model (default, depth 17):" << std::endl;
compare(map, PREDICTION_DEFAULT, iterations); compare(map.memory(), ids, likelihood, PREDICTION_DEFAULT, iterations);
std::cout << "[ ] 8 values model (depth 7, every value above 1e-4):" << std::endl; std::cout << "[ ] 8 values model (depth 7, every value above 1e-4):" << std::endl;
compare(map, PREDICTION_TRUNCATED, iterations); compare(map.memory(), ids, likelihood, PREDICTION_TRUNCATED, iterations);
} }
// While mapping, the graph changes on every node, so the matrix is kept for // Mapping mode, over a graph that has stopped growing: the matrix is kept, because
// updatePrediction() to carry its unchanged columns over and the sparse prediction is // updatePrediction() needs it to carry its unchanged columns over whenever the graph does
// taken from it rather than built instead of it. Same multiplication, no memory saved. // grow, and the sparse form is taken from it rather than built instead of it. It is worth
// taking here because the prediction outlasts an iteration, which is what
// DenseVsSparsePredictionWhileMappingARealSession does not have: there a location is added
// on every iteration and the sparse form is never built at all.
TEST(BayesFilterPerfTest, DenseVsSparsePredictionWhileMapping) TEST(BayesFilterPerfTest, DenseVsSparsePredictionWhileMapping)
{ {
const int nodes = 4000; const int nodes = 4000;
@@ -359,5 +580,143 @@ TEST(BayesFilterPerfTest, DenseVsSparsePredictionWhileMapping)
<< map.loopClosures() << " loop closures, dense matrix = " << map.loopClosures() << " loop closures, dense matrix = "
<< (size*size*sizeof(float))/1048576 << " MB" << std::endl; << (size*size*sizeof(float))/1048576 << " MB" << std::endl;
compare(map, PREDICTION_DEFAULT, iterations); const std::vector<int> ids = map.bayesIds();
compare(map.memory(), ids, map.uniformLikelihood(ids), PREDICTION_DEFAULT, iterations);
}
// The graphs of real maps, against the synthetic ones above, in localization mode where
// the graph is fixed. Two of them: one that went through the graph reduction, whose merged
// neighbor links cost a margin like ordinary neighbors, and one that did not.
//
// Needs the same ~1 GB as the largest synthetic map for the dense prediction, and reads
// the graphs from data/tests. Exclude with
// bin/test_bayesfilter_perf --gtest_filter=-*RealMap*
TEST(BayesFilterPerfTest, DenseVsSparsePredictionOnRealMaps)
{
const int iterations = 10;
if(!Optimizer::isAvailable(Optimizer::kTypeG2O))
{
GTEST_SKIP() << "g2o optimizer not built in, needed to read the graphs";
}
const char * files[] = {"large_reduced_graph.g2o", "large_mapping_session.g2o"};
const char * labels[] = {"graph reduction applied", "no graph reduction"};
for(size_t f=0; f<sizeof(files)/sizeof(const char *); ++f)
{
const std::string path = std::string(RTABMAP_TEST_DATA_ROOT) + "/tests/" + files[f];
if(!UFile::exists(path))
{
std::cout << "[ ] " << path << " not found, skipped" << std::endl;
continue;
}
RealGraph graph;
ASSERT_TRUE(loadRealGraph(path, graph)) << "could not read " << path;
Memory * memory = newRealMemory();
std::vector<int> newIds;
int removedLinks = 0;
UTimer timer;
for(size_t i=0; i<graph.ids.size(); ++i)
{
addRealNode(memory, graph, i, newIds, &removedLinks);
}
const double buildTime = timer.ticks();
ParametersMap localization;
localization.insert(ParametersPair(Parameters::kMemIncrementalMemory(), "false"));
memory->parseParameters(localization);
ASSERT_FALSE(memory->isIncremental());
const std::vector<int> ids = bayesIdsOf(memory);
std::cout << "[ ] " << files[f] << " (" << labels[f] << ")" << std::endl;
printGraph(path, graph, removedLinks);
std::cout << "[ ] rebuilt in " << buildTime << "s, " << ids.size()
<< " locations (with the virtual place), dense matrix = "
<< (ids.size()*ids.size()*sizeof(float))/1048576 << " MB" << std::endl;
const std::map<int, float> likelihood = uniformLikelihoodOf(ids);
std::cout << "[ ] 18 values model (default, depth 17):" << std::endl;
compare(memory, ids, likelihood, PREDICTION_DEFAULT, iterations, /*sparseFirst=*/true);
std::cout << "[ ] 8 values model (depth 7, every value above 1e-4):" << std::endl;
compare(memory, ids, likelihood, PREDICTION_TRUNCATED, iterations, /*sparseFirst=*/true);
delete memory;
}
}
// A mapping session as it runs: a location added, then an iteration of the filter, over and
// over. Every added location changes the prediction, so this is the case the sparse form
// cannot amortize -- unlike localization, where it is built once and reused for the rest of
// the session. What it costs to keep it up to date against what its multiplication saves is
// what this measures.
//
// The session is replayed from its end: the locations before the window are added without
// running the filter, so the per-location cost is measured at the size the map really
// reaches rather than at the sizes it passes through.
TEST(BayesFilterPerfTest, DenseVsSparsePredictionWhileMappingARealSession)
{
const size_t window = 15; // locations added one at a time, with an iteration each
if(!Optimizer::isAvailable(Optimizer::kTypeG2O))
{
GTEST_SKIP() << "g2o optimizer not built in, needed to read the graph";
}
const std::string path = std::string(RTABMAP_TEST_DATA_ROOT) + "/tests/large_mapping_session.g2o";
if(!UFile::exists(path))
{
GTEST_SKIP() << path << " not found";
}
RealGraph graph;
ASSERT_TRUE(loadRealGraph(path, graph)) << "could not read " << path;
ASSERT_GT(graph.ids.size(), window);
const size_t prepared = graph.ids.size() - window;
std::cout << "[ ] large_mapping_session.g2o, mapping mode: " << prepared
<< " locations already mapped, " << window
<< " more added one at a time with an iteration of the filter each" << std::endl;
std::map<int, float> lastPosterior[2];
for(int sparse=1; sparse>=0; --sparse) // the sparse mode first, see compare()
{
Memory * memory = newRealMemory();
std::vector<int> newIds;
int removedLinks = 0;
for(size_t i=0; i<prepared; ++i)
{
addRealNode(memory, graph, i, newIds, &removedLinks);
}
ParametersMap params;
params.insert(ParametersPair(Parameters::kBayesSparsePrediction(), sparse?"true":"false"));
BayesFilter filter(params);
// The first iteration generates the whole prediction, as it does at the start of a
// session; the ones after it are what a mapping session pays per location.
std::vector<int> ids = bayesIdsOf(memory);
UTimer timer;
filter.computePosterior(memory, uniformLikelihoodOf(ids));
const double first = timer.ticks();
double total = 0.0, best = 0.0, worst = 0.0;
for(size_t i=prepared; i<graph.ids.size(); ++i)
{
addRealNode(memory, graph, i, newIds, &removedLinks);
ids = bayesIdsOf(memory);
timer.restart();
filter.computePosterior(memory, uniformLikelihoodOf(ids));
const double elapsed = timer.ticks();
total += elapsed;
if(best == 0.0 || elapsed < best) best = elapsed;
if(elapsed > worst) worst = elapsed;
}
lastPosterior[sparse] = filter.getPosterior();
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",
sparse?"sparse":"dense", first*1000.0, best*1000.0, total*1000.0/double(window),
worst*1000.0, filter.getMemoryUsed()/1048576.0);
delete memory;
}
EXPECT_LT(maxPosteriorDifference(lastPosterior[0], lastPosterior[1]), 1e-3);
} }
corelib/test/test_bayesfilter.cpp
+13 -2
View File
@@ -1078,8 +1078,11 @@ TEST_F(BayesFilterMemoryFixture, SparsePredictionFallsBackWhenModelSumsBelowOne)
EXPECT_EQ(filterDense.getMemoryUsed(), filterSparse.getMemoryUsed()); EXPECT_EQ(filterDense.getMemoryUsed(), filterSparse.getMemoryUsed());
} }
// The sparse view holds only the non-zero values of the prediction matrix, so it is a // While the prediction matrix is being kept (mapping mode), its sparse form is only taken
// fraction of its size, and the reported memory reflects that it is an addition to it. // from it once the prediction outlasts an iteration: reading the whole matrix to build the
// form costs the work of one multiplication, so on a prediction that is multiplied once it
// would never be repaid. The sparse form then holds only the non-zero values, so it is a
// fraction of the matrix, and the reported memory reflects that it is an addition to it.
TEST_F(BayesFilterMemoryFixture, SparsePredictionMemoryUsed) TEST_F(BayesFilterMemoryFixture, SparsePredictionMemoryUsed)
{ {
addChain(40); addChain(40);
@@ -1097,6 +1100,14 @@ TEST_F(BayesFilterMemoryFixture, SparsePredictionMemoryUsed)
filterDense.computePosterior(memory_, likelihood); filterDense.computePosterior(memory_, likelihood);
filterSparse.computePosterior(memory_, likelihood); filterSparse.computePosterior(memory_, likelihood);
// The first iteration built the matrix, and nothing else: as far as this iteration
// knows, the prediction is about to be replaced by the next one.
EXPECT_EQ(filterDense.getMemoryUsed(), filterSparse.getMemoryUsed());
// The second finds the same prediction, so its sparse form is worth building.
filterDense.computePosterior(memory_, likelihood);
filterSparse.computePosterior(memory_, likelihood);
const unsigned long dense = filterDense.getMemoryUsed(); const unsigned long dense = filterDense.getMemoryUsed();
const unsigned long sparse = filterSparse.getMemoryUsed(); const unsigned long sparse = filterSparse.getMemoryUsed();
EXPECT_GT(sparse, dense); EXPECT_GT(sparse, dense);
corelib/test/test_graph.cpp
+138 -4
View File
@@ -8,6 +8,8 @@
#include <cmath> #include <cmath>
#include <string> #include <string>
#include <vector> #include <vector>
#include <fstream>
#include <sstream>
using namespace rtabmap; using namespace rtabmap;
@@ -922,10 +924,10 @@ void expectLinksNearEqual(
const auto idxB = index(b); const auto idxB = index(b);
ASSERT_EQ(idxA.size(), idxB.size()) ASSERT_EQ(idxA.size(), idxB.size())
<< label << " unique (from,to) link pair count differs"; << label << " unique (from,to) link pair count differs";
// Note: graph file formats (TORO / g2o) store edges generically and // Note: TORO's text format stores edges generically and doesn't preserve
// don't preserve rtabmap's Link::Type tag, so we only round-trip // rtabmap's Link::Type tag, so from / to / transform / infMatrix are all
// from / to / transform / infMatrix here. The loader assigns a // that round-trip there. g2o carries the type in a column of its own,
// placeholder type for ordinary edges. // which G2oRoundTripPreservesLinkTypes below checks.
// //
// Landmark links in g2o are written as EDGE_SE3_TRACKXYZ (3D point // Landmark links in g2o are written as EDGE_SE3_TRACKXYZ (3D point
// observation): only the translation and the 3x3 translation block // observation): only the translation and the 3x3 translation block
@@ -1260,3 +1262,135 @@ INSTANTIATE_TEST_SUITE_P(
+ "_" + "_"
+ (std::get<2>(info.param) ? "rotPrior" : "posPrior"); + (std::get<2>(info.param) ? "rotPrior" : "posPrior");
}); });
// -------------------------------------------------------------------------
// The type of a link (a loop closure against an odometry link, and which kind
// of loop closure) decides how the graph is traversed: Memory::getNeighborsId()
// follows a global closure without spending any depth, skips a proximity one
// and spends a depth on a neighbor. The g2o format defines no field for it, so
// OptimizerG2O writes it as a column past the ones it defines, which its own
// loader reads back and g2o's ignores.
//
// Also checks that a link handed over in both directions, which is how Memory
// stores it, is written once: g2o reads two lines as two constraints and would
// count the information of the link twice.
// -------------------------------------------------------------------------
TEST(GraphG2oTest, G2oRoundTripPreservesLinkTypes)
{
if(!Optimizer::isAvailable(Optimizer::kTypeG2O))
{
GTEST_SKIP() << "g2o optimizer not built in";
}
const Link::Type types[] = {
Link::kNeighbor,
Link::kNeighborMerged,
Link::kGlobalClosure,
Link::kLocalSpaceClosure,
Link::kLocalTimeClosure,
Link::kUserClosure,
};
const size_t typeCount = sizeof(types)/sizeof(Link::Type);
std::map<int, Transform> poses;
for(size_t i=0; i<=typeCount; ++i)
{
poses.insert(std::make_pair((int)i+1, Transform((float)i, 0.0f, 0.0f, 0, 0, 0)));
}
const cv::Mat infMatrix = cv::Mat::eye(6, 6, CV_64F) * 100.0;
std::multimap<int, Link> links;
for(size_t i=0; i<typeCount; ++i)
{
const int from = (int)i+1, to = (int)i+2;
const Transform t = poses.at(from).inverse() * poses.at(to);
// Both directions, as Memory holds them: one link stored on each of the
// two nodes it connects.
links.insert(std::make_pair(from, Link(from, to, types[i], t, infMatrix)));
links.insert(std::make_pair(to, Link(to, from, types[i], t.inverse(), infMatrix)));
}
const std::string path = test::tempPath(
uFormat("rtabmap_graph_link_types_%d.g2o", test::getPid()));
UFile::erase(path);
ASSERT_TRUE(graph::exportPoses(path, 4 /*g2o*/, poses, links));
ASSERT_TRUE(UFile::exists(path));
// One line per link, not two, and each one carrying its type last.
std::ifstream file(path.c_str());
std::string line;
std::map<std::pair<int,int>, int> written;
while(std::getline(file, line))
{
if(line.compare(0, 5, "EDGE_") != 0)
{
continue;
}
std::istringstream in(line);
std::string tag;
int from = 0, to = 0;
in >> tag >> from >> to;
std::string last;
while(in >> last) {}
const std::pair<int,int> pair(std::min(from,to), std::max(from,to));
EXPECT_TRUE(written.insert(std::make_pair(pair, atoi(last.c_str()))).second)
<< "link " << from << "->" << to << " written more than once";
}
ASSERT_EQ(written.size(), typeCount);
for(size_t i=0; i<typeCount; ++i)
{
EXPECT_EQ(written.at(std::make_pair((int)i+1, (int)i+2)), (int)types[i])
<< "type column of link " << i+1 << "->" << i+2;
}
// And read back as the types they were.
std::map<int, Transform> posesOut;
std::multimap<int, Link> linksOut;
ASSERT_TRUE(graph::importPoses(path, 4 /*g2o*/, posesOut, &linksOut));
ASSERT_EQ(linksOut.size(), typeCount);
std::map<std::pair<int,int>, Link::Type> loaded;
for(std::multimap<int, Link>::const_iterator iter=linksOut.begin(); iter!=linksOut.end(); ++iter)
{
loaded.insert(std::make_pair(
std::make_pair(std::min(iter->second.from(), iter->second.to()),
std::max(iter->second.from(), iter->second.to())),
iter->second.type()));
}
for(size_t i=0; i<typeCount; ++i)
{
const std::pair<int,int> pair((int)i+1, (int)i+2);
ASSERT_TRUE(loaded.find(pair) != loaded.end()) << "link " << i+1 << "->" << i+2 << " missing";
EXPECT_EQ(loaded.at(pair), types[i]) << "type of link " << i+1 << "->" << i+2;
}
UFile::erase(path);
}
// A file without the type column, which is every file g2o itself writes and
// every one rtabmap wrote before, still loads: the type stays the one its tag
// implies, as it did.
TEST(GraphG2oTest, G2oWithoutTypeColumnStillLoads)
{
if(!Optimizer::isAvailable(Optimizer::kTypeG2O))
{
GTEST_SKIP() << "g2o optimizer not built in";
}
const std::string path = test::tempPath(
uFormat("rtabmap_graph_no_type_column_%d.g2o", test::getPid()));
UFile::erase(path);
{
std::ofstream file(path.c_str());
file << "VERTEX_SE2 1 0 0 0\n";
file << "VERTEX_SE2 2 1 0 0\n";
file << "EDGE_SE2 1 2 1 0 0 100 0 0 100 0 100\n";
}
std::map<int, Transform> poses;
std::multimap<int, Link> links;
ASSERT_TRUE(graph::importPoses(path, 4 /*g2o*/, poses, &links));
EXPECT_EQ(poses.size(), 2u);
ASSERT_EQ(links.size(), 1u);
EXPECT_EQ(links.begin()->second.from(), 1);
EXPECT_EQ(links.begin()->second.to(), 2);
UFile::erase(path);
}
data/tests/large_mapping_session.g2o
+20982
View File
File diff suppressed because it is too large Load Diff
data/tests/large_reduced_graph.g2o
+57170
View File
File diff suppressed because it is too large Load Diff