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

Increased database version to 0.8.11 (new Depth.data2d_max_pts column). Updated how local loop closure detection in space is done. Update GraphViewer with local radius ellipse and current goal node color. MainWindow saving/loading figures automatically accordingly to the previous session saved.

Browse Source
This commit is contained in:
Mathieu Labbe
2015-05-03 18:16:55 -04:00
parent abb7eb15ac
commit d7030e0e38
28 changed files with 927 additions and 366 deletions
Download Patch File Download Diff File Expand all files Collapse all files

58
corelib/src/Graph.cpp
View File

@@ -1122,7 +1122,7 @@ public:
rtabmap::Transform pose() const {return pose_;}
float distFrom(const rtabmap::Transform & pose) const
{
return pose_.getDistanceSquared(pose); // use sqrt distance
return pose_.getDistance(pose); // use sqrt distance
}
void setClosed(bool closed) {closed_ = closed;}
@@ -1287,7 +1287,6 @@ int findNearestNode(
std::map<int, float> getNodesInRadius(
int nodeId,
const std::map<int, Transform> & nodes,
int maxNearestNeighbors,
float radius)
{
UASSERT(uContains(nodes, nodeId));
@@ -1323,7 +1322,7 @@ std::map<int, float> getNodesInRadius(
std::vector<int> ind;
std::vector<float> dist;
pcl::PointXYZ pt(fromT.x(), fromT.y(), fromT.z());
kdTree->radiusSearch(pt, radius, ind, dist, maxNearestNeighbors);
kdTree->radiusSearch(pt, radius, ind, dist, 0);
for(unsigned int i=0; i<ind.size(); ++i)
{
if(ind[i] >=0)
@@ -1337,6 +1336,59 @@ std::map<int, float> getNodesInRadius(
return foundNodes;
}
// return <id, Transform>, excluding query
std::map<int, Transform> getPosesInRadius(
int nodeId,
const std::map<int, Transform> & nodes,
float radius)
{
UASSERT(uContains(nodes, nodeId));
std::map<int, Transform> foundNodes;
if(nodes.size() <= 1)
{
return foundNodes;
}
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
cloud->resize(nodes.size());
std::vector<int> ids(nodes.size());
int oi = 0;
for(std::map<int, Transform>::const_iterator iter = nodes.begin(); iter!=nodes.end(); ++iter)
{
if(iter->first != nodeId)
{
(*cloud)[oi] = pcl::PointXYZ(iter->second.x(), iter->second.y(), iter->second.z());
UASSERT_MSG(pcl::isFinite((*cloud)[oi]), uFormat("Invalid pose (%d) %s", iter->first, iter->second.prettyPrint().c_str()).c_str());
ids[oi] = iter->first;
++oi;
}
}
cloud->resize(oi);
ids.resize(oi);
Transform fromT = nodes.at(nodeId);
if(cloud->size())
{
pcl::search::KdTree<pcl::PointXYZ>::Ptr kdTree(new pcl::search::KdTree<pcl::PointXYZ>);
kdTree->setInputCloud(cloud);
std::vector<int> ind;
std::vector<float> dist;
pcl::PointXYZ pt(fromT.x(), fromT.y(), fromT.z());
kdTree->radiusSearch(pt, radius, ind, dist, 0);
for(unsigned int i=0; i<ind.size(); ++i)
{
if(ind[i] >=0)
{
UDEBUG("Inlier %d: %f", ids[ind[i]], sqrt(dist[i]));
foundNodes.insert(std::make_pair(ids[ind[i]], nodes.at(ids[ind[i]])));
}
}
}
UDEBUG("found nodes=%d", (int)foundNodes.size());
return foundNodes;
}
float computePathLength(
const std::vector<std::pair<int, Transform> > & path,
unsigned int fromIndex,