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

Added Export Scans dialog

Browse Source
This commit is contained in:
matlabbe
2016-03-14 17:27:53 -04:00
parent 04df6ba41c
commit 5c92025742
14 changed files with 1251 additions and 311 deletions
Download Patch File Download Diff File Expand all files Collapse all files

53
corelib/src/util3d_filtering.cpp
View File

@@ -420,6 +420,14 @@ pcl::IndicesPtr radiusFiltering(
pcl::IndicesPtr indices(new std::vector<int>);
return radiusFiltering(cloud, indices, radiusSearch, minNeighborsInRadius);
}
pcl::IndicesPtr radiusFiltering(
const pcl::PointCloud<pcl::PointNormal>::Ptr & cloud,
float radiusSearch,
int minNeighborsInRadius)
{
pcl::IndicesPtr indices(new std::vector<int>);
return radiusFiltering(cloud, indices, radiusSearch, minNeighborsInRadius);
}
pcl::IndicesPtr radiusFiltering(
const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
float radiusSearch,
@@ -483,6 +491,51 @@ pcl::IndicesPtr radiusFiltering(
return output;
}
}
pcl::IndicesPtr radiusFiltering(
const pcl::PointCloud<pcl::PointNormal>::Ptr & cloud,
const pcl::IndicesPtr & indices,
float radiusSearch,
int minNeighborsInRadius)
{
pcl::search::KdTree<pcl::PointNormal>::Ptr tree (new pcl::search::KdTree<pcl::PointNormal>(false));
if(indices->size())
{
pcl::IndicesPtr output(new std::vector<int>(indices->size()));
int oi = 0; // output iterator
tree->setInputCloud(cloud, indices);
for(unsigned int i=0; i<indices->size(); ++i)
{
std::vector<int> kIndices;
std::vector<float> kDistances;
int k = tree->radiusSearch(cloud->at(indices->at(i)), radiusSearch, kIndices, kDistances);
if(k > minNeighborsInRadius)
{
output->at(oi++) = indices->at(i);
}
}
output->resize(oi);
return output;
}
else
{
pcl::IndicesPtr output(new std::vector<int>(cloud->size()));
int oi = 0; // output iterator
tree->setInputCloud(cloud);
for(unsigned int i=0; i<cloud->size(); ++i)
{
std::vector<int> kIndices;
std::vector<float> kDistances;
int k = tree->radiusSearch(cloud->at(i), radiusSearch, kIndices, kDistances);
if(k > minNeighborsInRadius)
{
output->at(oi++) = i;
}
}
output->resize(oi);
return output;
}
}
pcl::IndicesPtr radiusFiltering(
const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
const pcl::IndicesPtr & indices,

92
corelib/src/util3d_surface.cpp
View File

@@ -724,6 +724,52 @@ pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr mls(
return cloud_with_normals;
}
void adjustNormalsToViewPoints(
const std::map<int, Transform> & poses,
const pcl::PointCloud<pcl::PointXYZ>::Ptr & rawCloud,
const std::vector<int> & rawCameraIndices,
pcl::PointCloud<pcl::PointNormal>::Ptr & cloud)
{
if(poses.size() && rawCloud->size() && rawCloud->size() == rawCameraIndices.size() && cloud->size())
{
pcl::search::KdTree<pcl::PointXYZ>::Ptr rawTree (new pcl::search::KdTree<pcl::PointXYZ>);
rawTree->setInputCloud (rawCloud);
for(unsigned int i=0; i<cloud->size(); ++i)
{
pcl::PointXYZ normal(cloud->points[i].normal_x, cloud->points[i].normal_y, cloud->points[i].normal_z);
if(pcl::isFinite(normal))
{
std::vector<int> indices;
std::vector<float> dist;
rawTree->nearestKSearch(pcl::PointXYZ(cloud->points[i].x, cloud->points[i].y, cloud->points[i].z), 1, indices, dist);
UASSERT(indices.size() == 1);
if(indices.size() && indices[0]>=0)
{
Transform p = poses.at(rawCameraIndices[indices[0]]);
pcl::PointXYZ viewpoint(p.x(), p.y(), p.z());
Eigen::Vector3f v = viewpoint.getVector3fMap() - cloud->points[i].getVector3fMap();
Eigen::Vector3f n(normal.x, normal.y, normal.z);
float result = v.dot(n);
if(result < 0)
{
//reverse normal
cloud->points[i].normal_x *= -1.0f;
cloud->points[i].normal_y *= -1.0f;
cloud->points[i].normal_z *= -1.0f;
}
}
else
{
UWARN("Not found camera viewpoint for point %d", i);
}
}
}
}
}
void adjustNormalsToViewPoints(
const std::map<int, Transform> & poses,
const pcl::PointCloud<pcl::PointXYZ>::Ptr & rawCloud,
@@ -737,30 +783,34 @@ void adjustNormalsToViewPoints(
for(unsigned int i=0; i<cloud->size(); ++i)
{
std::vector<int> indices;
std::vector<float> dist;
rawTree->nearestKSearch(pcl::PointXYZ(cloud->points[i].x, cloud->points[i].y, cloud->points[i].z), 1, indices, dist);
UASSERT(indices.size() == 1);
if(indices.size() && indices[0]>=0)
pcl::PointXYZ normal(cloud->points[i].normal_x, cloud->points[i].normal_y, cloud->points[i].normal_z);
if(pcl::isFinite(normal))
{
Transform p = poses.at(rawCameraIndices[indices[0]]);
pcl::PointXYZ viewpoint(p.x(), p.y(), p.z());
Eigen::Vector3f v = viewpoint.getVector3fMap() - cloud->points[i].getVector3fMap();
Eigen::Vector3f n(cloud->points[i].normal_x, cloud->points[i].normal_y, cloud->points[i].normal_z);
float result = v.dot(n);
if(result < 0)
std::vector<int> indices;
std::vector<float> dist;
rawTree->nearestKSearch(pcl::PointXYZ(cloud->points[i].x, cloud->points[i].y, cloud->points[i].z), 1, indices, dist);
UASSERT(indices.size() == 1);
if(indices.size() && indices[0]>=0)
{
//reverse normal
cloud->points[i].normal_x *= -1.0f;
cloud->points[i].normal_y *= -1.0f;
cloud->points[i].normal_z *= -1.0f;
Transform p = poses.at(rawCameraIndices[indices[0]]);
pcl::PointXYZ viewpoint(p.x(), p.y(), p.z());
Eigen::Vector3f v = viewpoint.getVector3fMap() - cloud->points[i].getVector3fMap();
Eigen::Vector3f n(normal.x, normal.y, normal.z);
float result = v.dot(n);
if(result < 0)
{
//reverse normal
cloud->points[i].normal_x *= -1.0f;
cloud->points[i].normal_y *= -1.0f;
cloud->points[i].normal_z *= -1.0f;
}
}
else
{
UWARN("Not found camera viewpoint for point %d", i);
}
}
else
{
UWARN("Not found camera viewpoint for point %d", i);
}
}
}