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

The graph is also sent in appearance-only mode. Added posterior hypotheses color on the graph view. Some minor fixes.

Browse Source
This commit is contained in:
matlabbe
2015-01-07 17:30:20 -05:00
parent a979216fc1
commit 7f7d05e243
11 changed files with 309 additions and 198 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
corelib/src/DBDriverSqlite3.cpp
View File

@@ -1019,7 +1019,7 @@ void DBDriverSqlite3::loadSignaturesQuery(const std::list<int> & ids, std::list<
// create the node
if(id)
{
ULOGGER_DEBUG("Creating %d (map=%d)", *iter, mapId);
ULOGGER_DEBUG("Creating %d (map=%d, pose=%s)", *iter, mapId, pose.prettyPrint().c_str());
Signature * s = new Signature(
id,
mapId,

17
corelib/src/Memory.cpp
View File

@@ -704,11 +704,6 @@ void Memory::getPose(int locationId, Transform & pose, bool lookInDatabase) cons
{
pose = s->getPose();
mapId = s->mapId();
if(pose.isNull())
{
UERROR("Pose of %d is null?!?", locationId);
}
}
else if(lookInDatabase && _dbDriver)
{
@@ -1071,7 +1066,7 @@ std::map<int, float> Memory::computeLikelihood(const Signature * signature, cons
likelihood.insert(likelihood.end(), std::pair<int, float>(*iter, sim));
}
UDEBUG("compute likelihood... %f s", timer.ticks());
UDEBUG("compute likelihood (similarity)... %f s", timer.ticks());
return likelihood;
}
else
@@ -1147,7 +1142,7 @@ std::map<int, float> Memory::computeLikelihood(const Signature * signature, cons
}
}
UDEBUG("compute likelihood %f s", timer.ticks());
UDEBUG("compute likelihood (tf-idf) %f s", timer.ticks());
return likelihood;
}
}
@@ -3558,10 +3553,7 @@ void Memory::getMetricConstraints(
{
Transform pose;
this->getPose(ids[i], pose, lookInDatabase);
if(!pose.isNull())
{
poses.insert(std::make_pair(ids[i], pose));
}
poses.insert(std::make_pair(ids[i], pose));
}
for(unsigned int i=0; i<ids.size(); ++i)
@@ -3571,7 +3563,7 @@ void Memory::getMetricConstraints(
std::map<int, Link> neighbors = this->getNeighborLinks(ids[i], lookInDatabase); // only direct neighbors
for(std::map<int, Link>::iterator jter=neighbors.begin(); jter!=neighbors.end(); ++jter)
{
if(!jter->second.transform().isNull() && uContains(poses, jter->first))
if(uContains(poses, jter->first))
{
bool edgeAlreadyAdded = false;
for(std::multimap<int, Link>::iterator iter = links.lower_bound(jter->first);
@@ -3594,7 +3586,6 @@ void Memory::getMetricConstraints(
for(std::map<int, Link>::iterator jter=loops.begin(); jter!=loops.end(); ++jter)
{
if(jter->first < ids[i] &&
!jter->second.transform().isNull() &&
uContains(poses, jter->first))
{
links.insert(std::make_pair(ids[i],jter->second));

48
corelib/src/Rtabmap.cpp
View File

@@ -1664,20 +1664,33 @@ bool Rtabmap::process(const SensorData & data)
statistics_.addStatistic(Statistics::kMemorySignatures_removed(), signaturesRemoved.size());
//Poses, place this after Transfer! (_optimizedPoses may change)
std::map<int, int> mapIds;
if(_rgbdSlamMode)
{
std::map<int, int> mapIds;
for(std::map<int, Transform>::iterator iter=_optimizedPoses.begin(); iter!=_optimizedPoses.end(); ++iter)
{
mapIds.insert(std::make_pair(iter->first, _memory->getMapId(iter->first)));
}
statistics_.setMapIds(mapIds);
statistics_.setPoses(_optimizedPoses);
statistics_.setConstraints(_constraints);
statistics_.setMapCorrection(_mapCorrection);
UINFO("Set map correction = %s", _mapCorrection.prettyPrint().c_str());
}
else if(_memory->getLastWorkingSignature())
{
// no optimization on appearance-only mode
std::map<int, int> ids = _memory->getNeighborsId(_memory->getLastWorkingSignature()->id(), 0, 0, true);
std::map<int, Transform> poses;
std::multimap<int, Link> constraints;
_memory->getMetricConstraints(uKeys(ids), poses, constraints, false);
for(std::map<int, Transform>::iterator iter=poses.begin(); iter!=poses.end(); ++iter)
{
mapIds.insert(std::make_pair(iter->first, _memory->getMapId(iter->first)));
}
statistics_.setPoses(poses);
statistics_.setConstraints(constraints);
}
statistics_.setMapIds(mapIds);
}
// Log info...
@@ -2128,11 +2141,17 @@ void Rtabmap::get3DMap(std::map<int, Signature> & signatures,
std::map<int, int> ids = _memory->getNeighborsId(_memory->getLastWorkingSignature()->id(), 0, global?-1:0, true);
_memory->getMetricConstraints(uKeys(ids), poses, constraints, global);
}
}
else
{
// no optimization on appearance-only mode
std::map<int, int> ids = _memory->getNeighborsId(_memory->getLastWorkingSignature()->id(), 0, global?-1:0, true);
_memory->getMetricConstraints(uKeys(ids), poses, constraints, global);
}
for(std::map<int, Transform>::iterator iter=poses.begin(); iter!=poses.end(); ++iter)
{
mapIds.insert(std::make_pair(iter->first, _memory->getMapId(iter->first)));
}
for(std::map<int, Transform>::iterator iter=poses.begin(); iter!=poses.end(); ++iter)
{
mapIds.insert(std::make_pair(iter->first, _memory->getMapId(iter->first)));
}
@@ -2187,22 +2206,17 @@ void Rtabmap::getGraph(
std::map<int, int> ids = _memory->getNeighborsId(_memory->getLastWorkingSignature()->id(), 0, global?-1:0, true);
_memory->getMetricConstraints(uKeys(ids), poses, constraints, global);
}
for(std::map<int, Transform>::iterator iter=poses.begin(); iter!=poses.end(); ++iter)
{
mapIds.insert(std::make_pair(iter->first, _memory->getMapId(iter->first)));
}
}
else
{
UWARN("Getting graph while not in RGB-D SLAM mode makes no sense... only map ids are set.");
// no optimization on appearance-only mode
std::map<int, int> ids = _memory->getNeighborsId(_memory->getLastWorkingSignature()->id(), 0, global?-1:0, true);
_memory->getMetricConstraints(uKeys(ids), poses, constraints, global);
}
std::set<int> ids = _memory->getWorkingMem(); // STM + WM
ids.insert(_memory->getStMem().begin(), _memory->getStMem().end());
if(global)
for(std::map<int, Transform>::iterator iter=poses.begin(); iter!=poses.end(); ++iter)
{
ids = _memory->getAllSignatureIds(); // STM + WM + LTM
mapIds.insert(std::make_pair(iter->first, _memory->getMapId(iter->first)));
}
}
else if(_memory && (_memory->getStMem().size() || _memory->getWorkingMem().size()))

4
corelib/src/RtabmapThread.cpp
View File

@@ -174,6 +174,10 @@ void RtabmapThread::mainLoop()
break;
case kStateInit:
UASSERT(!parameters.at("RtabmapThread/DatabasePath").empty());
Parameters::parse(parameters, Parameters::kRtabmapImageBufferSize(), _dataBufferMaxSize);
Parameters::parse(parameters, Parameters::kRtabmapDetectionRate(), _rate);
UASSERT(_dataBufferMaxSize >= 0);
UASSERT(_rate >= 0.0f);
_rtabmap->init(parameters, parameters.at("RtabmapThread/DatabasePath"));
break;
case kStateChangingParameters:

6
corelib/src/util3d.cpp
View File

@@ -2350,6 +2350,7 @@ void optimizeTOROGraph(
{
if(uContains(depthGraph, iter->first))
{
UASSERT(!iter->second.isNull());
posesToro.insert(std::make_pair(rtabmapToToro.at(iter->first), iter->second));
}
}
@@ -2359,6 +2360,7 @@ void optimizeTOROGraph(
{
if(uContains(depthGraph, iter->second.from()) && uContains(depthGraph, iter->second.to()))
{
UASSERT(!iter->second.transform().isNull());
edgeConstraintsToro.insert(std::make_pair(rtabmapToToro.at(iter->first), Link(rtabmapToToro.at(iter->first), rtabmapToToro.at(iter->second.to()), iter->second.type(), iter->second.transform(), iter->second.variance())));
}
}
@@ -2433,6 +2435,7 @@ void optimizeTOROGraph(
for(std::map<int, Transform>::const_iterator iter = poses.begin(); iter!=poses.end(); ++iter)
{
float x,y,z, roll,pitch,yaw;
UASSERT(!iter->second.isNull());
pcl::getTranslationAndEulerAngles(transformToEigen3f(iter->second), x,y,z, roll,pitch,yaw);
AISNavigation::TreePoseGraph3::Pose p(x, y, z, roll, pitch, yaw);
AISNavigation::TreePoseGraph<AISNavigation::Operations3D<double> >::Vertex* v = pg.addVertex(iter->first, p);
@@ -2451,6 +2454,7 @@ void optimizeTOROGraph(
int id1 = iter->first;
int id2 = iter->second.to();
float x,y,z, roll,pitch,yaw;
UASSERT(!iter->second.transform().isNull());
pcl::getTranslationAndEulerAngles(transformToEigen3f(iter->second.transform()), x,y,z, roll,pitch,yaw);
AISNavigation::TreePoseGraph3::Pose p(x, y, z, roll, pitch, yaw);
AISNavigation::TreePoseGraph3::InformationMatrix inf = DMatrix<double>::I(6);
@@ -2939,6 +2943,7 @@ cv::Mat create2DMapFromOccupancyLocalMaps(
{
if(uContains(occupancy, iter->first))
{
UASSERT(!iter->second.isNull());
const std::pair<cv::Mat, cv::Mat> & pair = occupancy.at(iter->first);
iter->second.getTranslationAndEulerAngles(x,y,z,toll,pitch,yaw);
@@ -3100,6 +3105,7 @@ cv::Mat create2DMap(const std::map<int, Transform> & poses,
{
if(uContains(scans, iter->first) && scans.at(iter->first)->size())
{
UASSERT(!iter->second.isNull());
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud = transformPointCloud<pcl::PointXYZ>(scans.at(iter->first), iter->second);
pcl::PointXYZ min, max;
pcl::getMinMax3D(*cloud, min, max);