i/rtabmap
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap.git synced 2026-09-03 01:50:24 +08:00
Code Issues Packages Projects Releases Wiki Activity

util3d::laserScanFromPointCloud(...): ignore NaN points and normals. RegistrationIcp: ignoring NaNs on conversion to PD format to fix some registration fails caused by NaNs.

Browse Source
This commit is contained in:
matlabbe
2018-03-13 02:27:59 -04:00
parent 257fe20c4f
commit ecf598e412
2 changed files with 607 additions and 416 deletions
Download Patch File Download Diff File Expand all files Collapse all files

113
corelib/src/RegistrationIcp.cpp
View File

@@ -195,64 +195,97 @@ DP laserScanToDP(const rtabmap::LaserScan & scan)
View viewNormalY(nx!=-1?cloud.getDescriptorRowViewByName("normals",1):view); View viewNormalY(nx!=-1?cloud.getDescriptorRowViewByName("normals",1):view);
View viewNormalZ(nx!=-1?cloud.getDescriptorRowViewByName("normals",2):view); View viewNormalZ(nx!=-1?cloud.getDescriptorRowViewByName("normals",2):view);
View viewIntensity(offsetI!=-1?cloud.getDescriptorRowViewByName("intensity",0):view); View viewIntensity(offsetI!=-1?cloud.getDescriptorRowViewByName("intensity",0):view);
int oi = 0;
for(int i=0; i<scan.size(); ++i) for(int i=0; i<scan.size(); ++i)
{ {
const float * ptr = scan.data().ptr<float>(0, i); const float * ptr = scan.data().ptr<float>(0, i);
if(hasLocalTransform) if(uIsFinite(ptr[0]) && uIsFinite(ptr[1]) && (scan.is2d() || uIsFinite(ptr[2])))
{ {
if(nx == -1) if(hasLocalTransform)
{ {
cv::Point3f pt(ptr[0], ptr[1], scan.is2d()?0:ptr[2]); if(nx == -1)
pt = rtabmap::util3d::transformPoint(pt, scan.localTransform());
view(0, i) = pt.x;
view(1, i) = pt.y;
if(!scan.is2d())
{ {
view(2, i) = pt.z; cv::Point3f pt(ptr[0], ptr[1], scan.is2d()?0:ptr[2]);
pt = rtabmap::util3d::transformPoint(pt, scan.localTransform());
view(0, oi) = pt.x;
view(1, oi) = pt.y;
if(!scan.is2d())
{
view(2, oi) = pt.z;
}
if(offsetI!=-1)
{
viewIntensity(0, oi) = ptr[offsetI];
}
++oi;
}
else if(uIsFinite(ptr[nx]) && uIsFinite(ptr[ny]) && uIsFinite(ptr[nz]))
{
pcl::PointNormal pt;
pt.x=ptr[0];
pt.y=ptr[1];
pt.z=scan.is2d()?0:ptr[2];
pt.normal_x=ptr[nx];
pt.normal_y=ptr[ny];
pt.normal_z=ptr[nz];
pt = rtabmap::util3d::transformPoint(pt, scan.localTransform());
view(0, oi) = pt.x;
view(1, oi) = pt.y;
if(!scan.is2d())
{
view(2, oi) = pt.z;
}
viewNormalX(0, oi) = pt.normal_x;
viewNormalY(0, oi) = pt.normal_y;
viewNormalZ(0, oi) = pt.normal_z;
if(offsetI!=-1)
{
viewIntensity(0, oi) = ptr[offsetI];
}
++oi;
}
else
{
UWARN("Ignoring point %d with invalid data: pos=%f %f %f, normal=%f %f %f", i, ptr[0], ptr[1], scan.is2d()?0:ptr[3], ptr[nx], ptr[ny], ptr[nz]);
} }
} }
else if(uIsFinite(ptr[nx]) && uIsFinite(ptr[ny]) && uIsFinite(ptr[nz])) else if(nx==-1 || (uIsFinite(ptr[nx]) && uIsFinite(ptr[ny]) && uIsFinite(ptr[nz])))
{ {
pcl::PointNormal pt; view(0, oi) = ptr[0];
pt.x=ptr[0]; view(1, oi) = ptr[1];
pt.y=ptr[1];
pt.z=scan.is2d()?0:ptr[2];
pt.normal_x=ptr[nx];
pt.normal_y=ptr[ny];
pt.normal_z=ptr[nz];
pt = rtabmap::util3d::transformPoint(pt, scan.localTransform());
view(0, i) = pt.x;
view(1, i) = pt.y;
if(!scan.is2d()) if(!scan.is2d())
{ {
view(2, i) = pt.z; view(2, oi) = ptr[2];
} }
viewNormalX(0, i) = pt.normal_x; if(nx!=-1)
viewNormalY(0, i) = pt.normal_y; {
viewNormalZ(0, i) = pt.normal_z; viewNormalX(0, oi) = ptr[nx];
viewNormalY(0, oi) = ptr[ny];
viewNormalZ(0, oi) = ptr[nz];
}
if(offsetI!=-1)
{
viewIntensity(0, oi) = ptr[offsetI];
}
++oi;
}
else
{
UWARN("Ignoring point %d with invalid data: pos=%f %f %f, normal=%f %f %f", i, ptr[0], ptr[1], scan.is2d()?0:ptr[3], ptr[nx], ptr[ny], ptr[nz]);
} }
} }
else if(nx!=-1 || (uIsFinite(ptr[nx]) && uIsFinite(ptr[ny]) && uIsFinite(ptr[nz]))) else
{ {
view(0, i) = ptr[0]; UWARN("Ignoring point %d with invalid data: pos=%f %f %f", i, ptr[0], ptr[1], scan.is2d()?0:ptr[3]);
view(1, i) = ptr[1];
if(!scan.is2d())
{
view(2, i) = ptr[2];
}
if(nx!=-1)
{
viewNormalX(0, i) = ptr[nx];
viewNormalY(0, i) = ptr[ny];
viewNormalZ(0, i) = ptr[nz];
}
} }
if(offsetI!=-1) }
{ if(oi != scan.size())
viewIntensity(0, i) = ptr[offsetI]; {
} cloud.conservativeResize(oi);
} }
return cloud; return cloud;

910
corelib/src/util3d.cpp
View File

File diff suppressed because it is too large Load Diff