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

Fixed #1413

Browse Source
This commit is contained in:
matlabbe
2024-12-15 17:51:15 -08:00
parent 97c56361f8
commit 02e30ffc69
2 changed files with 5 additions and 5 deletions
Download Patch File Download Diff File Expand all files Collapse all files

6
corelib/src/util3d_filtering.cpp
View File

@@ -2155,7 +2155,7 @@ pcl::IndicesPtr normalFilteringImpl(
for(unsigned int i=0; i<cloud_normals->size(); ++i)
{
Eigen::Vector4f v(cloud_normals->at(i).normal_x, cloud_normals->at(i).normal_y, cloud_normals->at(i).normal_z, 0.0f);
if(groundNormalsUp>0.0f && v[2] < -groundNormalsUp && cloud->at(indices->size()!=0?indices->at(i):i).z < viewpoint[3]) // some far velodyne rays on road can have normals toward ground
if(groundNormalsUp>0.0f && v[2] < -groundNormalsUp && cloud->at(indices->size()!=0?indices->at(i):i).z < viewpoint[2]) // some far velodyne rays on road can have normals toward ground
{
//reverse normal
v *= -1.0f;
@@ -2226,7 +2226,7 @@ pcl::IndicesPtr normalFilteringImpl(
for(unsigned int i=0; i<indices->size(); ++i)
{
Eigen::Vector4f v(cloud->at(indices->at(i)).normal_x, cloud->at(indices->at(i)).normal_y, cloud->at(indices->at(i)).normal_z, 0.0f);
if(groundNormalsUp>0.0f && v[2] < -groundNormalsUp && cloud->at(indices->at(i)).z < viewpoint[3]) // some far velodyne rays on road can have normals toward ground
if(groundNormalsUp>0.0f && v[2] < -groundNormalsUp && cloud->at(indices->at(i)).z < viewpoint[2]) // some far velodyne rays on road can have normals toward ground
{
//reverse normal
v *= -1.0f;
@@ -2244,7 +2244,7 @@ pcl::IndicesPtr normalFilteringImpl(
for(unsigned int i=0; i<cloud->size(); ++i)
{
Eigen::Vector4f v(cloud->at(i).normal_x, cloud->at(i).normal_y, cloud->at(i).normal_z, 0.0f);
if(groundNormalsUp>0.0f && v[2] < -groundNormalsUp && cloud->at(i).z < viewpoint[3]) // some far velodyne rays on road can have normals toward ground
if(groundNormalsUp>0.0f && v[2] < -groundNormalsUp && cloud->at(i).z < viewpoint[2]) // some far velodyne rays on road can have normals toward ground
{
//reverse normal
v *= -1.0f;

4
corelib/src/util3d_surface.cpp
View File

@@ -3530,7 +3530,7 @@ LaserScan adjustNormalsToViewPoint(
float result = v.dot(n);
if(result < 0
|| (groundNormalsUp>0.0f && ptr[nz] < -groundNormalsUp && ptr[2] < viewpoint[3])) // some far velodyne rays on road can have normals toward ground
|| (groundNormalsUp>0.0f && ptr[nz] < -groundNormalsUp && ptr[2] < viewpoint[2])) // some far velodyne rays on road can have normals toward ground
{
//reverse normal
ptr[nx] *= -1.0f;
@@ -3569,7 +3569,7 @@ void adjustNormalsToViewPointImpl(
float result = v.dot(n);
if(result < 0
|| (groundNormalsUp>0.0f && normal.z < -groundNormalsUp && cloud->points[i].z < viewpoint[3])) // some far velodyne rays on road can have normals toward ground
|| (groundNormalsUp>0.0f && normal.z < -groundNormalsUp && cloud->points[i].z < viewpoint[2])) // some far velodyne rays on road can have normals toward ground
{
//reverse normal
cloud->points[i].normal_x *= -1.0f;