i/rtabmap
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap.git synced 2026-09-02 17:40:23 +08:00
Code Issues Packages Projects Releases Wiki Activity

Rtabmap: reset all parameters to default on close()

Browse Source
This commit is contained in:
Mathieu Labbe
2015-03-20 15:16:08 -04:00
parent ad6ee69ac6
commit 10eee94e98
2 changed files with 174 additions and 165 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
corelib/include/rtabmap/core/Rtabmap.h
View File

@@ -197,7 +197,7 @@ private:
EpipolarGeometry * _epipolarGeometry; EpipolarGeometry * _epipolarGeometry;
BayesFilter * _bayesFilter; BayesFilter * _bayesFilter;
graph::Optimizer * _graphOptimizer; graph::Optimizer * _graphOptimizer;
ParametersMap _lastParameters; ParametersMap _modifiedParameters;
Memory * _memory; Memory * _memory;

337
corelib/src/Rtabmap.cpp
View File

@@ -265,6 +265,8 @@ void Rtabmap::init(const ParametersMap & parameters, const std::string & databas
{ {
_databasePath = _wDir + "/" + Parameters::getDefaultDatabaseName(); _databasePath = _wDir + "/" + Parameters::getDefaultDatabaseName();
} }
UASSERT(UFile::getExtension(_databasePath).compare("db") == 0);
UINFO("Using database \"%s\".", _databasePath.c_str()); UINFO("Using database \"%s\".", _databasePath.c_str());
bool newDatabase = !UFile::exists(_databasePath); bool newDatabase = !UFile::exists(_databasePath);
this->parseParameters(parameters); this->parseParameters(parameters);
@@ -330,16 +332,12 @@ void Rtabmap::close()
_graphOptimizer = 0; _graphOptimizer = 0;
} }
_databasePath.clear(); _databasePath.clear();
parseParameters(Parameters::getDefaultParameters()); // reset to default parameters
_modifiedParameters.clear();
} }
void Rtabmap::parseParameters(const ParametersMap & parameters) void Rtabmap::parseParameters(const ParametersMap & parameters)
{ {
if(_databasePath.empty())
{
UERROR("RTAB-Map is not initialized. Call rtabmap::init() instead.");
return;
}
ULOGGER_DEBUG(""); ULOGGER_DEBUG("");
ParametersMap::const_iterator iter; ParametersMap::const_iterator iter;
if((iter=parameters.find(Parameters::kRtabmapWorkingDirectory())) != parameters.end()) if((iter=parameters.find(Parameters::kRtabmapWorkingDirectory())) != parameters.end())
@@ -422,8 +420,11 @@ void Rtabmap::parseParameters(const ParametersMap & parameters)
if(!_memory) if(!_memory)
{ {
_memory = new Memory(parameters); if(!_databasePath.empty())
_memory->init(_databasePath, false, parameters, true); {
_memory = new Memory(parameters);
_memory->init(_databasePath, false, parameters, true);
}
} }
else else
{ {
@@ -462,7 +463,7 @@ void Rtabmap::parseParameters(const ParametersMap & parameters)
for(ParametersMap::const_iterator iter = parameters.begin(); iter!=parameters.end(); ++iter) for(ParametersMap::const_iterator iter = parameters.begin(); iter!=parameters.end(); ++iter)
{ {
uInsert(_lastParameters, ParametersPair(iter->first, iter->second)); uInsert(_modifiedParameters, ParametersPair(iter->first, iter->second));
} }
} }
@@ -700,10 +701,11 @@ void Rtabmap::resetMemory()
_constraints.clear(); _constraints.clear();
_mapCorrection.setIdentity(); _mapCorrection.setIdentity();
_mapTransform.setIdentity(); _mapTransform.setIdentity();
this->clearPath();
if(_memory) if(_memory)
{ {
_memory->init(_databasePath, true, ParametersMap(), true); _memory->init(_databasePath, true, _modifiedParameters, true);
if(_memory->getLastWorkingSignature()) if(_memory->getLastWorkingSignature())
{ {
optimizeCurrentMap(_memory->getLastWorkingSignature()->id(), false, _optimizedPoses, &_constraints); optimizeCurrentMap(_memory->getLastWorkingSignature()->id(), false, _optimizedPoses, &_constraints);
@@ -1378,7 +1380,7 @@ bool Rtabmap::process(const SensorData & data)
std::string rejectedMsg; std::string rejectedMsg;
if(_reextractLoopClosureFeatures) if(_reextractLoopClosureFeatures)
{ {
ParametersMap customParameters = _lastParameters; // get BOW LCC parameters ParametersMap customParameters = _modifiedParameters; // get BOW LCC parameters
// override some parameters // override some parameters
uInsert(customParameters, ParametersPair(Parameters::kMemIncrementalMemory(), "true")); // make sure it is incremental uInsert(customParameters, ParametersPair(Parameters::kMemIncrementalMemory(), "true")); // make sure it is incremental
uInsert(customParameters, ParametersPair(Parameters::kMemRehearsalSimilarity(), "1.0")); // desactivate rehearsal uInsert(customParameters, ParametersPair(Parameters::kMemRehearsalSimilarity(), "1.0")); // desactivate rehearsal
@@ -2053,115 +2055,118 @@ std::map<int, Transform> Rtabmap::getWMPosesInRadius(
int maxDiffID, // 0 means ignore int maxDiffID, // 0 means ignore
int & nearestId) const int & nearestId) const
{ {
UDEBUG("");
const Signature * fromS = _memory->getSignature(fromId);
UASSERT(fromS != 0);
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
cloud->resize(_optimizedPoses.size());
std::vector<int> ids(_optimizedPoses.size());
int oi = 0;
const std::set<int> & stm = _memory->getStMem();
//get margins
std::map<int, int> margins;
if(maxDiffID > 0)
{
margins = _memory->getNeighborsId(fromId, maxDiffID, 0, true, false);
}
for(std::map<int, Transform>::const_iterator iter = _optimizedPoses.begin(); iter!=_optimizedPoses.end(); ++iter)
{
// Only locations in Working Memory not too far from the current node (so inside the margin)
bool diffIdOk = maxDiffID == 0 || uContains(margins, iter->first);
if(stm.find(iter->first) == stm.end() && diffIdOk)
{
(*cloud)[oi] = pcl::PointXYZ(iter->second.x(), iter->second.y(), iter->second.z());
ids[oi++] = iter->first;
}
}
cloud->resize(oi);
ids.resize(oi);
UASSERT(_optimizedPoses.find(fromId) != _optimizedPoses.end());
Transform fromT = _optimizedPoses.at(fromId);
nearestId = 0;
std::map<int, Transform> poses; std::map<int, Transform> poses;
float minDistance = -1; if(_memory && fromId > 0)
if(cloud->size())
{ {
//if(cloud->size()) UDEBUG("");
//{ const Signature * fromS = _memory->getSignature(fromId);
// pcl::io::savePCDFile("radiusPoses.pcd", *cloud); UASSERT(fromS != 0);
// UWARN("Saved radiusPoses.pcd");
//}
//filter poses in front of the fromId pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
Transform t=Transform::getIdentity(); cloud->resize(_optimizedPoses.size());
t.x() = radius*0.95f; std::vector<int> ids(_optimizedPoses.size());
float x,y,z, roll,pitch,yaw; int oi = 0;
(fromT*t).getTranslationAndEulerAngles(x,y,z, roll,pitch,yaw); const std::set<int> & stm = _memory->getStMem();
//get margins
pcl::CropBox<pcl::PointXYZ> cropbox; std::map<int, int> margins;
cropbox.setInputCloud(cloud); if(maxDiffID > 0)
cropbox.setMin(Eigen::Vector4f(-radius, -radius, -radius, 0));
cropbox.setMax(Eigen::Vector4f(radius, radius, radius, 0));
cropbox.setRotation(Eigen::Vector3f(roll, pitch, yaw));
cropbox.setTranslation(Eigen::Vector3f(x, y, z));
pcl::IndicesPtr indices(new std::vector<int>());
cropbox.filter(*indices);
//if(indices->size())
//{
// pcl::io::savePCDFile("radiusCrop.pcd", *cloud, *indices);
// UWARN("Saved radiusCrop.pcd");
//}
if(indices->size())
{ {
pcl::search::KdTree<pcl::PointXYZ>::Ptr kdTree(new pcl::search::KdTree<pcl::PointXYZ>); margins = _memory->getNeighborsId(fromId, maxDiffID, 0, true, false);
kdTree->setInputCloud(cloud, indices); }
std::vector<int> ind; for(std::map<int, Transform>::const_iterator iter = _optimizedPoses.begin(); iter!=_optimizedPoses.end(); ++iter)
std::vector<float> dist; {
pcl::PointXYZ pt(fromT.x(), fromT.y(), fromT.z()); // Only locations in Working Memory not too far from the current node (so inside the margin)
kdTree->radiusSearch(pt, radius, ind, dist, maxNearestNeighbors); bool diffIdOk = maxDiffID == 0 || uContains(margins, iter->first);
//pcl::PointCloud<pcl::PointXYZ> inliers; if(stm.find(iter->first) == stm.end() && diffIdOk)
for(unsigned int i=0; i<ind.size(); ++i)
{ {
if(ind[i] >=0) (*cloud)[oi] = pcl::PointXYZ(iter->second.x(), iter->second.y(), iter->second.z());
ids[oi++] = iter->first;
}
}
cloud->resize(oi);
ids.resize(oi);
UASSERT(_optimizedPoses.find(fromId) != _optimizedPoses.end());
Transform fromT = _optimizedPoses.at(fromId);
nearestId = 0;
float minDistance = -1;
if(cloud->size())
{
//if(cloud->size())
//{
// pcl::io::savePCDFile("radiusPoses.pcd", *cloud);
// UWARN("Saved radiusPoses.pcd");
//}
//filter poses in front of the fromId
Transform t=Transform::getIdentity();
t.x() = radius*0.95f;
float x,y,z, roll,pitch,yaw;
(fromT*t).getTranslationAndEulerAngles(x,y,z, roll,pitch,yaw);
pcl::CropBox<pcl::PointXYZ> cropbox;
cropbox.setInputCloud(cloud);
cropbox.setMin(Eigen::Vector4f(-radius, -radius, -radius, 0));
cropbox.setMax(Eigen::Vector4f(radius, radius, radius, 0));
cropbox.setRotation(Eigen::Vector3f(roll, pitch, yaw));
cropbox.setTranslation(Eigen::Vector3f(x, y, z));
pcl::IndicesPtr indices(new std::vector<int>());
cropbox.filter(*indices);
//if(indices->size())
//{
// pcl::io::savePCDFile("radiusCrop.pcd", *cloud, *indices);
// UWARN("Saved radiusCrop.pcd");
//}
if(indices->size())
{
pcl::search::KdTree<pcl::PointXYZ>::Ptr kdTree(new pcl::search::KdTree<pcl::PointXYZ>);
kdTree->setInputCloud(cloud, indices);
std::vector<int> ind;
std::vector<float> dist;
pcl::PointXYZ pt(fromT.x(), fromT.y(), fromT.z());
kdTree->radiusSearch(pt, radius, ind, dist, maxNearestNeighbors);
//pcl::PointCloud<pcl::PointXYZ> inliers;
for(unsigned int i=0; i<ind.size(); ++i)
{ {
Transform tmp = _optimizedPoses.find(ids[ind[i]])->second; if(ind[i] >=0)
//inliers.push_back(pcl::PointXYZ(tmp.x(), tmp.y(), tmp.z()));
UDEBUG("Inlier %d: %s", ids[ind[i]], tmp.prettyPrint().c_str());
poses.insert(std::make_pair(ids[ind[i]], tmp));
if(minDistance == -1 || minDistance > dist[i])
{ {
nearestId = ids[ind[i]]; Transform tmp = _optimizedPoses.find(ids[ind[i]])->second;
minDistance = dist[i]; //inliers.push_back(pcl::PointXYZ(tmp.x(), tmp.y(), tmp.z()));
UDEBUG("Inlier %d: %s", ids[ind[i]], tmp.prettyPrint().c_str());
poses.insert(std::make_pair(ids[ind[i]], tmp));
if(minDistance == -1 || minDistance > dist[i])
{
nearestId = ids[ind[i]];
minDistance = dist[i];
}
} }
} }
}
//if(inliers.size()) //if(inliers.size())
//{ //{
// pcl::io::savePCDFile("radiusInliers.pcd", inliers); // pcl::io::savePCDFile("radiusInliers.pcd", inliers);
//} //}
//if(nearestId >0) //if(nearestId >0)
//{ //{
// pcl::PointCloud<pcl::PointXYZ> c; // pcl::PointCloud<pcl::PointXYZ> c;
// Transform ct = _optimizedPoses.find(nearestId)->second; // Transform ct = _optimizedPoses.find(nearestId)->second;
// c.push_back(pcl::PointXYZ(ct.x(), ct.y(), ct.z())); // c.push_back(pcl::PointXYZ(ct.x(), ct.y(), ct.z()));
// pcl::io::savePCDFile("radiusNearestPt.pcd", c); // pcl::io::savePCDFile("radiusNearestPt.pcd", c);
//} //}
if(nearestId == 0 && poses.size()) if(nearestId == 0 && poses.size())
{ {
UWARN("Flushing poses (%d) because nearest id of %d can't be found!", (int)poses.size(), fromId); UWARN("Flushing poses (%d) because nearest id of %d can't be found!", (int)poses.size(), fromId);
poses.clear(); poses.clear();
}
} }
} }
UDEBUG("nearestId = %d, minDistance=%f poses=%d", nearestId, minDistance, (int)poses.size());
} }
UDEBUG("nearestId = %d, minDistance=%f poses=%d", nearestId, minDistance, (int)poses.size());
return poses; return poses;
} }
@@ -2461,67 +2466,71 @@ bool Rtabmap::computePath(
const std::map<int, Transform> & nodes, const std::map<int, Transform> & nodes,
const std::multimap<int, rtabmap::Link> & constraints) const std::multimap<int, rtabmap::Link> & constraints)
{ {
if(!_memory->getLastWorkingSignature()) if(_memory)
{ {
UWARN("Working memory is empty... cannot compute a path"); if(!_memory->getLastWorkingSignature())
return false;
}
int currentNode = _memory->getLastWorkingSignature()->id();
if(!uContains(nodes, currentNode))
{
UWARN("Last signature %d not found in the graph! Cannot compute a path", currentNode);
return false;
}
if(!uContains(nodes, targetNode))
{
UWARN("Goal %d not found in the graph! Cannot compute a path", targetNode);
return false;
}
std::multimap<int, int> links;
for(std::multimap<int, rtabmap::Link>::const_iterator iter=constraints.begin(); iter!=constraints.end(); ++iter)
{
links.insert(std::make_pair(iter->first, iter->second.to()));
links.insert(std::make_pair(iter->second.to(), iter->first)); // <->
}
// Add links between neighbor nodes in the goal radius.
if(_planWithNearNodesLinked)
{
std::multimap<int, int> clusters = rtabmap::graph::radiusPosesClustering(nodes, _goalReachedRadius, CV_PI);
links.insert(clusters.begin(), clusters.end());
}
UINFO("Computing path from location %d to %d", currentNode, targetNode);
UTimer timer;
_path = uListToVector(rtabmap::graph::computePath(nodes, links, currentNode, targetNode));
UINFO("A* time = %fs", timer.ticks());
if(_path.size() == 0)
{
_path.clear();
UWARN("Cannot compute a path!");
}
else
{
UINFO("Path generated! Size=%d", (int)_path.size());
if(ULogger::level() == ULogger::kInfo)
{ {
std::stringstream stream; UWARN("Working memory is empty... cannot compute a path");
for(unsigned int i=0; i<_path.size(); ++i) return false;
{
stream << _path[i].first;
if(i+1 < _path.size())
{
stream << " ";
}
}
UINFO("Path = [%s]", stream.str().c_str());
} }
} int currentNode = _memory->getLastWorkingSignature()->id();
return _path.size()>0; if(!uContains(nodes, currentNode))
{
UWARN("Last signature %d not found in the graph! Cannot compute a path", currentNode);
return false;
}
if(!uContains(nodes, targetNode))
{
UWARN("Goal %d not found in the graph! Cannot compute a path", targetNode);
return false;
}
std::multimap<int, int> links;
for(std::multimap<int, rtabmap::Link>::const_iterator iter=constraints.begin(); iter!=constraints.end(); ++iter)
{
links.insert(std::make_pair(iter->first, iter->second.to()));
links.insert(std::make_pair(iter->second.to(), iter->first)); // <->
}
// Add links between neighbor nodes in the goal radius.
if(_planWithNearNodesLinked)
{
std::multimap<int, int> clusters = rtabmap::graph::radiusPosesClustering(nodes, _goalReachedRadius, CV_PI);
links.insert(clusters.begin(), clusters.end());
}
UINFO("Computing path from location %d to %d", currentNode, targetNode);
UTimer timer;
_path = uListToVector(rtabmap::graph::computePath(nodes, links, currentNode, targetNode));
UINFO("A* time = %fs", timer.ticks());
if(_path.size() == 0)
{
_path.clear();
UWARN("Cannot compute a path!");
}
else
{
UINFO("Path generated! Size=%d", (int)_path.size());
if(ULogger::level() == ULogger::kInfo)
{
std::stringstream stream;
for(unsigned int i=0; i<_path.size(); ++i)
{
stream << _path[i].first;
if(i+1 < _path.size())
{
stream << " ";
}
}
UINFO("Path = [%s]", stream.str().c_str());
}
}
return _path.size()>0;
}
return false;
} }
// return true if path is updated // return true if path is updated
@@ -2670,7 +2679,7 @@ void Rtabmap::updateGoalIndex()
return; return;
} }
if(_path.size()) if( _memory && _path.size())
{ {
// Make sure the next signatures on the path are linked together // Make sure the next signatures on the path are linked together
float distanceSoFar = 0.0f; float distanceSoFar = 0.0f;