i/rtabmap_ros
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap_ros.git synced 2026-10-03 16:27:46 +08:00
Code Issues Packages Projects Releases Wiki Activity

💄

Browse Source
This commit is contained in:
matlabbe
2015-08-24 21:02:32 -04:00
parent fcdc82daae
commit 0ac79d0e8f
3 changed files with 7 additions and 35 deletions
Download Patch File Download Diff File Expand all files Collapse all files
corelib/include/rtabmap/core/util3d_surface.h
+1 -2
View File
@@ -83,8 +83,7 @@ void RTABMAP_EXP adjustNormalsToViewPoints(
const std::map<int, Transform> & poses,
const pcl::PointCloud<pcl::PointXYZ>::Ptr & rawCloud,
const std::vector<int> & rawCameraIndices,
pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & cloud,
int k = 0); // optional: recompute normal with k neighbors (min k=3)
pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & cloud);
pcl::PolygonMesh::Ptr RTABMAP_EXP meshDecimation(const pcl::PolygonMesh::Ptr & mesh, float factor);
corelib/src/util3d_surface.cpp
+5 -22
View File
@@ -327,17 +327,13 @@ void adjustNormalsToViewPoints(
const std::map<int, Transform> & poses,
const pcl::PointCloud<pcl::PointXYZ>::Ptr & rawCloud,
const std::vector<int> & rawCameraIndices,
pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & cloud,
int k)
pcl::PointCloud<pcl::PointXYZRGBNormal>::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);
pcl::search::KdTree<pcl::PointXYZRGBNormal>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZRGBNormal>);
tree->setInputCloud (cloud);
for(unsigned int i=0; i<cloud->size(); ++i)
{
std::vector<int> indices;
@@ -350,23 +346,6 @@ void adjustNormalsToViewPoints(
pcl::PointXYZ viewpoint(p.x(), p.y(), p.z());
Eigen::Vector3f v = viewpoint.getVector3fMap() - cloud->points[i].getVector3fMap();
//compute point normal
if(k >= 3)
{
tree->nearestKSearch(cloud->points[i], k, indices, dist);
if(indices.size() >= 3)
{
Eigen::Vector4f planeParameters;
float curvature;
pcl::computePointNormal(*cloud, indices, planeParameters, curvature);
//update normal
cloud->points[i].normal_x = planeParameters[0];
cloud->points[i].normal_y = planeParameters[1];
cloud->points[i].normal_z = planeParameters[2];
}
}
Eigen::Vector3f n(cloud->points[i].normal_x, cloud->points[i].normal_y, cloud->points[i].normal_z);
float result = v.dot(n);
@@ -378,6 +357,10 @@ void adjustNormalsToViewPoints(
cloud->points[i].normal_z *= -1.0f;
}
}
else
{
UWARN("Not found camera viewpoint for point %d", i);
}
}
}
}
guilib/src/MainWindow.cpp
+1 -11
View File
@@ -4720,21 +4720,11 @@ bool MainWindow::getExportedClouds(
if(_exportDialog->getAssemble())
{
_initProgressDialog->appendText(tr("Update %1 normals with %2 camera views...").arg(cloudWithNormals->size()).arg(poses.size()));
pcl::PointCloud<pcl::PointXYZ>::Ptr viewpoints(new pcl::PointCloud<pcl::PointXYZ>);
viewpoints->resize(poses.size());
int oi=0;
for(std::map<int, Transform>::const_iterator iter=poses.begin(); iter!=poses.end(); ++iter)
{
(*viewpoints)[oi].x = iter->second.x();
(*viewpoints)[oi].y = iter->second.y();
(*viewpoints)[oi++].z = iter->second.z();
}
util3d::adjustNormalsToViewPoints(
poses,
rawAssembledCloud,
rawCameraIndices,
cloudWithNormals,
_exportDialog->getNormalKSearch());
cloudWithNormals);
}
cloudsWithNormals.insert(std::make_pair(iter->first, cloudWithNormals));