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

merged master to multicamera branch

Browse Source
This commit is contained in:
Mathieu Labbe
2015-05-29 14:54:49 -04:00
parent c6d0d47b1c ec8946dc2f
commit c5046df226
9 changed files with 53 additions and 59 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -123,8 +123,8 @@ public:
void getGraph(std::map<int, Transform> & poses,
std::multimap<int, Link> & constraints,
bool optimized,
bool global,
std::map<int, Signature> * signatures = 0);
bool global,
std::map<int, Signature> * signatures = 0);
void clearPath();
bool computePath(int targetNode, bool global);
bool computePath(const Transform & targetPose, bool global);

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

@@ -96,7 +96,7 @@ private:
void pushNewState(State newState, const ParametersMap & parameters = ParametersMap());
void setDataBufferSize(int size);
void publishMap(bool optimized, bool full) const;
void publishTOROGraph(bool optimized, bool full) const;
void publishGraph(bool optimized, bool full) const;
private:
UMutex _stateMutex;

9
corelib/src/DBReader.cpp
View File

@@ -185,8 +185,6 @@ void DBReader::mainLoop()
if(goalId > 0)
{
this->post(new RtabmapEventCmd(RtabmapEventCmd::kCmdGoal, "", goalId));
if(!_ignoreGoalDelay && _currentId != _ids.end())
{
// get stamp for the next signature to compute the delay
@@ -203,12 +201,19 @@ void DBReader::mainLoop()
double delay = stamp - previousStamp;
UWARN("Goal %d detected, posting it! Waiting %f seconds before sending next data...",
goalId, delay);
this->post(new RtabmapEventCmd(RtabmapEventCmd::kCmdGoal, "", goalId));
uSleep(delay*1000);
}
else
{
UWARN("Goal %d detected, posting it!", goalId);
this->post(new RtabmapEventCmd(RtabmapEventCmd::kCmdGoal, "", goalId));
}
}
else
{
UWARN("Goal %d detected, posting it!", goalId);
this->post(new RtabmapEventCmd(RtabmapEventCmd::kCmdGoal, "", goalId));
}
}

37
corelib/src/Rtabmap.cpp
View File

@@ -2562,17 +2562,16 @@ void Rtabmap::optimizeCurrentMap(
{
//Optimize the map
optimizedPoses.clear();
UDEBUG("Optimize map: around location %d", id);
UINFO("Optimize map: around location %d", id);
if(_memory && id > 0)
{
UTimer timer;
std::map<int, int> ids = _memory->getNeighborsId(id, 0, lookInDatabase?-1:0, true);
UDEBUG("get ids=%d", (int)ids.size());
if(!_optimizeFromGraphEnd && ids.size() > 1)
{
id = ids.begin()->first;
}
UINFO("get ids time %f s", timer.ticks());
UINFO("get %d ids time %f s", (int)ids.size(), timer.ticks());
optimizedPoses = Rtabmap::optimizeGraph(id, uKeysSet(ids), lookInDatabase, constraints);
@@ -2597,7 +2596,7 @@ std::map<int, Transform> Rtabmap::optimizeGraph(
std::multimap<int, Link> edgeConstraints;
UDEBUG("ids=%d", (int)ids.size());
_memory->getMetricConstraints(ids, poses, edgeConstraints, lookInDatabase);
UDEBUG("get constraints (%d poses, %d edges) time %f s", (int)poses.size(), (int)edgeConstraints.size(), timer.ticks());
UINFO("get constraints (%d poses, %d edges) time %f s", (int)poses.size(), (int)edgeConstraints.size(), timer.ticks());
if(constraints)
{
@@ -2614,6 +2613,7 @@ std::map<int, Transform> Rtabmap::optimizeGraph(
{
optimizedPoses = _graphOptimizer->optimize(fromId, poses, edgeConstraints);
}
UINFO("Optimization time %f s", timer.ticks());
return optimizedPoses;
}
@@ -2801,8 +2801,8 @@ void Rtabmap::getGraph(
std::map<int, Transform> & poses,
std::multimap<int, Link> & constraints,
bool optimized,
bool global,
std::map<int, Signature> * signatures)
bool global,
std::map<int, Signature> * signatures)
{
if(_memory && _memory->getLastWorkingSignature())
{
@@ -2824,8 +2824,8 @@ void Rtabmap::getGraph(
std::map<int, int> ids = _memory->getNeighborsId(_memory->getLastWorkingSignature()->id(), 0, global?-1:0, true);
_memory->getMetricConstraints(uKeysSet(ids), poses, constraints, global);
}
if(signatures)
if(signatures)
{
for(std::map<int, Transform>::iterator iter=poses.begin(); iter!=poses.end(); ++iter)
{
@@ -2834,8 +2834,8 @@ void Rtabmap::getGraph(
int mapId = -1;
std::string label;
double stamp = 0;
std::vector<unsigned char> userData;
_memory->getNodeInfo(iter->first, odomPose, mapId, weight, label, stamp, userData, true);
std::vector<unsigned char> userData;
_memory->getNodeInfo(iter->first, odomPose, mapId, weight, label, stamp, userData, global);
signatures->insert(std::make_pair(iter->first,
Signature(iter->first,
mapId,
@@ -2846,7 +2846,7 @@ void Rtabmap::getGraph(
std::multimap<int, pcl::PointXYZ>(),
odomPose,
userData,
SensorData())));
SensorData())));
}
}
}
@@ -2986,6 +2986,7 @@ bool Rtabmap::computePath(
// return true if path is updated
bool Rtabmap::computePath(int targetNode, bool global)
{
UINFO("Planning a path to node %d (global=%d)", targetNode, global?1:0);
this->clearPath();
if(!_rgbdSlamMode)
@@ -2994,23 +2995,27 @@ bool Rtabmap::computePath(int targetNode, bool global)
return false;
}
UTimer totalTimer;
UTimer timer;
std::map<int, Transform> nodes;
std::multimap<int, Link> constraints;
this->getGraph(nodes, constraints, true, global);
std::multimap<int, Link> constraints;
this->getGraph(nodes, constraints, true, global);
UINFO("Time creating graph (global=%s) = %fs", global?"true":"false", timer.ticks());
if(computePath(targetNode, nodes, constraints))
{
updateGoalIndex();
}
UINFO("Time computing path = %fs", timer.ticks());
UINFO("Time computing path (A*) = %fs", timer.ticks());
UINFO("Total planning time = %fs (%d nodes, %f m long)", totalTimer.ticks(), (int)_path.size(), graph::computePathLength(_path));
return _path.size()>0;
}
bool Rtabmap::computePath(const Transform & targetPose, bool global)
{
UINFO("Planning a path to pose %s (global=%d)", targetPose.prettyPrint().c_str(), global?1:0);
this->clearPath();
std::list<std::pair<int, Transform> > pathPoses;
@@ -3027,8 +3032,8 @@ bool Rtabmap::computePath(const Transform & targetPose, bool global)
std::map<int, int> mapIds;
std::map<int, double> stamps;
std::map<int, std::string> labels;
std::map<int, std::vector<unsigned char> > userDatas;
this->getGraph(nodes, constraints, true, global);
std::map<int, std::vector<unsigned char> > userDatas;
this->getGraph(nodes, constraints, true, global);
UINFO("Time creating graph (global=%s) = %fs", global?"true":"false", timer.ticks());
int nearestId = rtabmap::graph::findNearestNode(nodes, targetPose);

6
corelib/src/RtabmapThread.cpp
View File

@@ -134,7 +134,7 @@ void RtabmapThread::publishMap(bool optimized, bool full) const
constraints));
}
void RtabmapThread::publishTOROGraph(bool optimized, bool full) const
void RtabmapThread::publishGraph(bool optimized, bool full) const
{
std::map<int, Signature> signatures;
std::map<int, Transform> poses;
@@ -244,10 +244,10 @@ void RtabmapThread::mainLoop()
this->publishMap(atoi(parameters.at("optimized").c_str())!=0, true);
break;
case kStatePublishingTOROGraphLocal:
this->publishTOROGraph(atoi(parameters.at("optimized").c_str())!=0, false);
this->publishGraph(atoi(parameters.at("optimized").c_str())!=0, false);
break;
case kStatePublishingTOROGraphGlobal:
this->publishTOROGraph(atoi(parameters.at("optimized").c_str())!=0, true);
this->publishGraph(atoi(parameters.at("optimized").c_str())!=0, true);
break;
case kStateTriggeringMap:
_rtabmap->triggerNewMap();

4
corelib/src/util2d.cpp
View File

@@ -228,7 +228,7 @@ float getDepth(
int u_end = std::min(u+1, depthImage.cols-1);
int v_end = std::min(v+1, depthImage.rows-1);
float depth = isInMM?(float)depthImage.at<uint16_t>(v,u)*0.001f:depthImage.at<float>(v,u);
float depth = isInMM?(float)depthImage.at<unsigned short>(v,u)*0.001f:depthImage.at<float>(v,u);
if(depth!=0.0f && uIsFinite(depth))
{
if(smoothing)
@@ -241,7 +241,7 @@ float getDepth(
{
if(!(uu == u && vv == v))
{
float d = isInMM?(float)depthImage.at<uint16_t>(vv,uu)*0.001f:depthImage.at<float>(vv,uu);
float d = isInMM?(float)depthImage.at<unsigned short>(vv,uu)*0.001f:depthImage.at<float>(vv,uu);
// ignore if not valid or depth difference is too high
if(d != 0.0f && uIsFinite(d) && fabs(d - depth) < maxZError)
{

34
corelib/src/util3d_filtering.cpp
View File

@@ -52,10 +52,8 @@ pcl::PointCloud<pcl::PointXYZ>::Ptr voxelize(
const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
float voxelSize)
{
typedef pcl::PointCloud<pcl::PointXYZ> PointCloud;
typedef typename PointCloud::Ptr PointCloudPtr;
UASSERT(voxelSize > 0.0f);
PointCloudPtr output(new PointCloud);
pcl::PointCloud<pcl::PointXYZ>::Ptr output(new pcl::PointCloud<pcl::PointXYZ>);
pcl::VoxelGrid<pcl::PointXYZ> filter;
filter.setLeafSize(voxelSize, voxelSize, voxelSize);
filter.setInputCloud(cloud);
@@ -66,10 +64,8 @@ pcl::PointCloud<pcl::PointXYZRGB>::Ptr voxelize(
const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
float voxelSize)
{
typedef pcl::PointCloud<pcl::PointXYZRGB> PointCloud;
typedef typename PointCloud::Ptr PointCloudPtr;
UASSERT(voxelSize > 0.0f);
PointCloudPtr output(new PointCloud);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr output(new pcl::PointCloud<pcl::PointXYZRGB>);
pcl::VoxelGrid<pcl::PointXYZRGB> filter;
filter.setLeafSize(voxelSize, voxelSize, voxelSize);
filter.setInputCloud(cloud);
@@ -81,10 +77,8 @@ pcl::PointCloud<pcl::PointXYZRGB>::Ptr voxelize(
pcl::PointCloud<pcl::PointXYZ>::Ptr sampling(
const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud, int samples)
{
typedef pcl::PointCloud<pcl::PointXYZ> PointCloud;
typedef typename PointCloud::Ptr PointCloudPtr;
UASSERT(samples > 0);
PointCloudPtr output(new PointCloud);
pcl::PointCloud<pcl::PointXYZ>::Ptr output(new pcl::PointCloud<pcl::PointXYZ>);
pcl::RandomSample<pcl::PointXYZ> filter;
filter.setSample(samples);
filter.setInputCloud(cloud);
@@ -94,10 +88,8 @@ pcl::PointCloud<pcl::PointXYZ>::Ptr sampling(
pcl::PointCloud<pcl::PointXYZRGB>::Ptr sampling(
const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud, int samples)
{
typedef pcl::PointCloud<pcl::PointXYZRGB> PointCloud;
typedef typename PointCloud::Ptr PointCloudPtr;
UASSERT(samples > 0);
PointCloudPtr output(new PointCloud);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr output(new pcl::PointCloud<pcl::PointXYZRGB>);
pcl::RandomSample<pcl::PointXYZRGB> filter;
filter.setSample(samples);
filter.setInputCloud(cloud);
@@ -112,12 +104,10 @@ pcl::PointCloud<pcl::PointXYZ>::Ptr passThrough(
float min,
float max)
{
typedef pcl::PointCloud<pcl::PointXYZ> PointCloud;
typedef typename PointCloud::Ptr PointCloudPtr;
UASSERT(max > min);
UASSERT(axis.compare("x") == 0 || axis.compare("y") == 0 || axis.compare("z") == 0);
PointCloudPtr output(new PointCloud);
pcl::PointCloud<pcl::PointXYZ>::Ptr output(new pcl::PointCloud<pcl::PointXYZ>);
pcl::PassThrough<pcl::PointXYZ> filter;
filter.setFilterFieldName(axis);
filter.setFilterLimits(min, max);
@@ -132,12 +122,10 @@ pcl::PointCloud<pcl::PointXYZRGB>::Ptr passThrough(
float min,
float max)
{
typedef pcl::PointCloud<pcl::PointXYZRGB> PointCloud;
typedef typename PointCloud::Ptr PointCloudPtr;
UASSERT(max > min);
UASSERT(axis.compare("x") == 0 || axis.compare("y") == 0 || axis.compare("z") == 0);
PointCloudPtr output(new PointCloud);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr output(new pcl::PointCloud<pcl::PointXYZRGB>);
pcl::PassThrough<pcl::PointXYZRGB> filter;
filter.setFilterFieldName(axis);
filter.setFilterLimits(min, max);
@@ -208,9 +196,7 @@ pcl::IndicesPtr radiusFiltering(
float radiusSearch,
int minNeighborsInRadius)
{
typedef pcl::search::KdTree<pcl::PointXYZ> KdTree;
typedef typename KdTree::Ptr KdTreePtr;
KdTreePtr tree (new KdTree(false));
pcl::search::KdTree<pcl::PointXYZ>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZ>(false));
if(indices->size())
{
@@ -255,9 +241,7 @@ pcl::IndicesPtr radiusFiltering(
float radiusSearch,
int minNeighborsInRadius)
{
typedef pcl::search::KdTree<pcl::PointXYZRGB> KdTree;
typedef typename KdTree::Ptr KdTreePtr;
KdTreePtr tree (new KdTree(false));
pcl::search::KdTree<pcl::PointXYZRGB>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZRGB>(false));
if(indices->size())
{
@@ -583,7 +567,7 @@ pcl::IndicesPtr extractNegativeIndices(
return output;
}
pcl::IndicesPtr extractNegativeIndices(
const typename pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
const pcl::IndicesPtr & indices)
{
pcl::IndicesPtr output(new std::vector<int>);

12
corelib/src/util3d_transforms.cpp
View File

@@ -36,22 +36,18 @@ namespace util3d
{
pcl::PointCloud<pcl::PointXYZ>::Ptr transformPointCloud(
const typename pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
const Transform & transform)
{
typedef pcl::PointCloud<pcl::PointXYZ> PointCloud;
typedef PointCloud::Ptr PointCloudPtr;
PointCloudPtr output(new PointCloud);
pcl::PointCloud<pcl::PointXYZ>::Ptr output(new pcl::PointCloud<pcl::PointXYZ>);
pcl::transformPointCloud(*cloud, *output, transform.toEigen4f());
return output;
}
pcl::PointCloud<pcl::PointXYZRGB>::Ptr transformPointCloud(
const typename pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
const Transform & transform)
{
typedef pcl::PointCloud<pcl::PointXYZRGB> PointCloud;
typedef PointCloud::Ptr PointCloudPtr;
PointCloudPtr output(new PointCloud);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr output(new pcl::PointCloud<pcl::PointXYZRGB>);
pcl::transformPointCloud(*cloud, *output, transform.toEigen4f());
return output;
}