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

Tango: fixed optimized mesh where nans were removed before saving (causing trouble with corresponding polygons)

Browse Source
This commit is contained in:
matlabbe
2018-03-21 16:00:12 -04:00
parent b0b3b491a0
commit 8d93c275ab
4 changed files with 91 additions and 84 deletions
Download Patch File Download Diff File Expand all files Collapse all files
app/android/jni/RTABMapApp.cpp
+11 -9
View File
@@ -440,7 +440,7 @@ int RTABMapApp::openDatabase(const std::string & databasePath, bool databaseInMe
} }
{ {
LOGI("Creating the meshes (%d)....", poses.size()); LOGI("Creating the meshes (%d)....", (int)poses.size());
boost::mutex::scoped_lock lock(meshesMutex_); boost::mutex::scoped_lock lock(meshesMutex_);
createdMeshes_.clear(); createdMeshes_.clear();
int i=0; int i=0;
@@ -767,7 +767,7 @@ std::vector<pcl::Vertices> RTABMapApp::filterOrganizedPolygons(
unsigned int biggestClusterSize = 0; unsigned int biggestClusterSize = 0;
for(std::map<int, std::list<int> >::iterator iter=clusters.begin(); iter!=clusters.end(); ++iter) for(std::map<int, std::list<int> >::iterator iter=clusters.begin(); iter!=clusters.end(); ++iter)
{ {
LOGD("cluster %d = %d", iter->first, iter->second.size()); LOGD("cluster %d = %d", iter->first, (int)iter->second.size());
if(iter->second.size() > biggestClusterSize) if(iter->second.size() > biggestClusterSize)
{ {
@@ -1092,7 +1092,6 @@ int RTABMapApp::Render()
mesh.texCoords = optMesh_->tex_coordinates[0]; mesh.texCoords = optMesh_->tex_coordinates[0];
mesh.texture = optTexture_; mesh.texture = optTexture_;
} }
main_scene_.addMesh(g_optMeshId, mesh, opengl_world_T_rtabmap_world, true); main_scene_.addMesh(g_optMeshId, mesh, opengl_world_T_rtabmap_world, true);
} }
else else
@@ -2250,11 +2249,12 @@ bool RTABMapApp::exportMesh(
++iter) ++iter)
{ {
std::map<int, Mesh>::iterator jter = createdMeshes_.find(iter->first); std::map<int, Mesh>::iterator jter = createdMeshes_.find(iter->first);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud; pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZRGB>);
pcl::IndicesPtr indices(new std::vector<int>); pcl::IndicesPtr indices(new std::vector<int>);
rtabmap::CameraModel model; rtabmap::CameraModel model;
cv::Mat depth; cv::Mat depth;
float gains[3] = {1.0f}; float gains[3];
gains[0] = gains[1] = gains[2] = 1.0f;
if(jter != createdMeshes_.end()) if(jter != createdMeshes_.end())
{ {
cloud = jter->second.cloud; cloud = jter->second.cloud;
@@ -2373,7 +2373,7 @@ bool RTABMapApp::exportMesh(
poisson.setDepth(optimizedDepth); poisson.setDepth(optimizedDepth);
poisson.setInputCloud(mergedClouds); poisson.setInputCloud(mergedClouds);
poisson.reconstruct(*mesh); poisson.reconstruct(*mesh);
LOGI("Mesh reconstruction... done! %fs (%d polygons)", timer.ticks(), mesh->polygons.size()); LOGI("Mesh reconstruction... done! %fs (%d polygons)", timer.ticks(), (int)mesh->polygons.size());
if(progressionStatus_.isCanceled()) if(progressionStatus_.isCanceled())
{ {
@@ -2704,7 +2704,7 @@ bool RTABMapApp::exportMesh(
// save in database // save in database
pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZRGBNormal>); pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZRGBNormal>);
pcl::fromPCLPointCloud2(polygonMesh->cloud, *cloud); pcl::fromPCLPointCloud2(polygonMesh->cloud, *cloud);
cv::Mat cloudMat = rtabmap::compressData2(rtabmap::util3d::laserScanFromPointCloud(*cloud)); // for database cv::Mat cloudMat = rtabmap::compressData2(rtabmap::util3d::laserScanFromPointCloud(*cloud, rtabmap::Transform(), false)); // for database
std::vector<std::vector<std::vector<unsigned int> > > polygons(1); std::vector<std::vector<std::vector<unsigned int> > > polygons(1);
polygons[0].resize(polygonMesh->polygons.size()); polygons[0].resize(polygonMesh->polygons.size());
for(unsigned int p=0; p<polygonMesh->polygons.size(); ++p) for(unsigned int p=0; p<polygonMesh->polygons.size(); ++p)
@@ -2712,6 +2712,7 @@ bool RTABMapApp::exportMesh(
polygons[0][p] = polygonMesh->polygons[p].vertices; polygons[0][p] = polygonMesh->polygons[p].vertices;
} }
boost::mutex::scoped_lock lock(rtabmapMutex_); boost::mutex::scoped_lock lock(rtabmapMutex_);
rtabmap_->getMemory()->saveOptimizedMesh(cloudMat, poses, polygons); rtabmap_->getMemory()->saveOptimizedMesh(cloudMat, poses, polygons);
success = true; success = true;
} }
@@ -2720,7 +2721,7 @@ bool RTABMapApp::exportMesh(
{ {
pcl::PointCloud<pcl::PointNormal>::Ptr cloud(new pcl::PointCloud<pcl::PointNormal>); pcl::PointCloud<pcl::PointNormal>::Ptr cloud(new pcl::PointCloud<pcl::PointNormal>);
pcl::fromPCLPointCloud2(textureMesh->cloud, *cloud); pcl::fromPCLPointCloud2(textureMesh->cloud, *cloud);
cv::Mat cloudMat = rtabmap::compressData2(rtabmap::util3d::laserScanFromPointCloud(*cloud)); // for database cv::Mat cloudMat = rtabmap::compressData2(rtabmap::util3d::laserScanFromPointCloud(*cloud, rtabmap::Transform(), false)); // for database
// save in database // save in database
std::vector<std::vector<std::vector<unsigned int> > > polygons(textureMesh->tex_polygons.size()); std::vector<std::vector<std::vector<unsigned int> > > polygons(textureMesh->tex_polygons.size());
@@ -2756,7 +2757,8 @@ bool RTABMapApp::exportMesh(
std::map<int, Mesh>::iterator jter=createdMeshes_.find(iter->first); std::map<int, Mesh>::iterator jter=createdMeshes_.find(iter->first);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZRGB>); pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZRGB>);
pcl::IndicesPtr indices(new std::vector<int>); pcl::IndicesPtr indices(new std::vector<int>);
float gains[3] = {1.0f}; float gains[3];
gains[0] = gains[1] = gains[2] = 1.0f;
if(regenerateCloud) if(regenerateCloud)
{ {
if(jter != createdMeshes_.end()) if(jter != createdMeshes_.end())
corelib/include/rtabmap/core/util3d.h
+21 -21
View File
@@ -196,37 +196,37 @@ pcl::PointCloud<pcl::PointXYZ> RTABMAP_EXP laserScanFromDepthImages(
float maxDepth, float maxDepth,
float minDepth); float minDepth);
LaserScan RTABMAP_EXP laserScanFromPointCloud(const pcl::PCLPointCloud2 & cloud); LaserScan RTABMAP_EXP laserScanFromPointCloud(const pcl::PCLPointCloud2 & cloud, bool filterNaNs = true);
// return CV_32FC3 (x,y,z) // return CV_32FC3 (x,y,z)
cv::Mat RTABMAP_EXP laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZ> & cloud, const Transform & transform = Transform()); cv::Mat RTABMAP_EXP laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZ> & cloud, const Transform & transform = Transform(), bool filterNaNs = true);
cv::Mat RTABMAP_EXP laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZ> & cloud, const pcl::IndicesPtr & indices, const Transform & transform = Transform()); cv::Mat RTABMAP_EXP laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZ> & cloud, const pcl::IndicesPtr & indices, const Transform & transform = Transform(), bool filterNaNs = true);
// return CV_32FC6 (x,y,z,normal_x,normal_y,normal_z) // return CV_32FC6 (x,y,z,normal_x,normal_y,normal_z)
cv::Mat RTABMAP_EXP laserScanFromPointCloud(const pcl::PointCloud<pcl::PointNormal> & cloud, const Transform & transform = Transform()); cv::Mat RTABMAP_EXP laserScanFromPointCloud(const pcl::PointCloud<pcl::PointNormal> & cloud, const Transform & transform = Transform(), bool filterNaNs = true);
cv::Mat RTABMAP_EXP laserScanFromPointCloud(const pcl::PointCloud<pcl::PointNormal> & cloud, const pcl::IndicesPtr & indices, const Transform & transform = Transform()); cv::Mat RTABMAP_EXP laserScanFromPointCloud(const pcl::PointCloud<pcl::PointNormal> & cloud, const pcl::IndicesPtr & indices, const Transform & transform = Transform(), bool filterNaNs = true);
cv::Mat RTABMAP_EXP laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZ> & cloud, const pcl::PointCloud<pcl::Normal> & normals, const Transform & transform = Transform()); cv::Mat RTABMAP_EXP laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZ> & cloud, const pcl::PointCloud<pcl::Normal> & normals, const Transform & transform = Transform(), bool filterNaNs = true);
// return CV_32FC4 (x,y,z,rgb) // return CV_32FC4 (x,y,z,rgb)
cv::Mat RTABMAP_EXP laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZRGB> & cloud, const Transform & transform = Transform()); cv::Mat RTABMAP_EXP laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZRGB> & cloud, const Transform & transform = Transform(), bool filterNaNs = true);
cv::Mat RTABMAP_EXP laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZRGB> & cloud, const pcl::IndicesPtr & indices, const Transform & transform = Transform()); cv::Mat RTABMAP_EXP laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZRGB> & cloud, const pcl::IndicesPtr & indices, const Transform & transform = Transform(), bool filterNaNs = true);
// return CV_32FC4 (x,y,z,I) // return CV_32FC4 (x,y,z,I)
cv::Mat RTABMAP_EXP laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZI> & cloud, const Transform & transform = Transform()); cv::Mat RTABMAP_EXP laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZI> & cloud, const Transform & transform = Transform(), bool filterNaNs = true);
cv::Mat RTABMAP_EXP laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZI> & cloud, const pcl::IndicesPtr & indices, const Transform & transform = Transform()); cv::Mat RTABMAP_EXP laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZI> & cloud, const pcl::IndicesPtr & indices, const Transform & transform = Transform(), bool filterNaNs = true);
// return CV_32FC7 (x,y,z,rgb,normal_x,normal_y,normal_z) // return CV_32FC7 (x,y,z,rgb,normal_x,normal_y,normal_z)
cv::Mat RTABMAP_EXP laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZRGB> & cloud, const pcl::PointCloud<pcl::Normal> & normals, const Transform & transform = Transform()); cv::Mat RTABMAP_EXP laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZRGB> & cloud, const pcl::PointCloud<pcl::Normal> & normals, const Transform & transform = Transform(), bool filterNaNs = true);
cv::Mat RTABMAP_EXP laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZRGBNormal> & cloud, const Transform & transform = Transform()); cv::Mat RTABMAP_EXP laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZRGBNormal> & cloud, const Transform & transform = Transform(), bool filterNaNs = true);
cv::Mat RTABMAP_EXP laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZRGBNormal> & cloud, const pcl::IndicesPtr & indices, const Transform & transform = Transform()); cv::Mat RTABMAP_EXP laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZRGBNormal> & cloud, const pcl::IndicesPtr & indices, const Transform & transform = Transform(), bool filterNaNs = true);
// return CV_32FC7 (x,y,z,I,normal_x,normal_y,normal_z) // return CV_32FC7 (x,y,z,I,normal_x,normal_y,normal_z)
cv::Mat RTABMAP_EXP laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZI> & cloud, const pcl::PointCloud<pcl::Normal> & normals, const Transform & transform = Transform()); cv::Mat RTABMAP_EXP laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZI> & cloud, const pcl::PointCloud<pcl::Normal> & normals, const Transform & transform = Transform(), bool filterNaNs = true);
cv::Mat RTABMAP_EXP laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZINormal> & cloud, const Transform & transform = Transform()); cv::Mat RTABMAP_EXP laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZINormal> & cloud, const Transform & transform = Transform(), bool filterNaNs = true);
// return CV_32FC2 (x,y) // return CV_32FC2 (x,y)
cv::Mat RTABMAP_EXP laserScan2dFromPointCloud(const pcl::PointCloud<pcl::PointXYZ> & cloud, const Transform & transform = Transform()); cv::Mat RTABMAP_EXP laserScan2dFromPointCloud(const pcl::PointCloud<pcl::PointXYZ> & cloud, const Transform & transform = Transform(), bool filterNaNs = true);
// return CV_32FC3 (x,y,I) // return CV_32FC3 (x,y,I)
cv::Mat RTABMAP_EXP laserScan2dFromPointCloud(const pcl::PointCloud<pcl::PointXYZI> & cloud, const Transform & transform = Transform()); cv::Mat RTABMAP_EXP laserScan2dFromPointCloud(const pcl::PointCloud<pcl::PointXYZI> & cloud, const Transform & transform = Transform(), bool filterNaNs = true);
// return CV_32FC5 (x,y,normal_x, normal_y, normal_z) // return CV_32FC5 (x,y,normal_x, normal_y, normal_z)
cv::Mat RTABMAP_EXP laserScan2dFromPointCloud(const pcl::PointCloud<pcl::PointNormal> & cloud, const Transform & transform = Transform()); cv::Mat RTABMAP_EXP laserScan2dFromPointCloud(const pcl::PointCloud<pcl::PointNormal> & cloud, const Transform & transform = Transform(), bool filterNaNs = true);
cv::Mat RTABMAP_EXP laserScan2dFromPointCloud(const pcl::PointCloud<pcl::PointXYZ> & cloud, const pcl::PointCloud<pcl::Normal> & normals, const Transform & transform = Transform()); cv::Mat RTABMAP_EXP laserScan2dFromPointCloud(const pcl::PointCloud<pcl::PointXYZ> & cloud, const pcl::PointCloud<pcl::Normal> & normals, const Transform & transform = Transform(), bool filterNaNs = true);
// return CV_32FC6 (x,y,I,normal_x, normal_y, normal_z) // return CV_32FC6 (x,y,I,normal_x, normal_y, normal_z)
cv::Mat RTABMAP_EXP laserScan2dFromPointCloud(const pcl::PointCloud<pcl::PointXYZINormal> & cloud, const Transform & transform = Transform()); cv::Mat RTABMAP_EXP laserScan2dFromPointCloud(const pcl::PointCloud<pcl::PointXYZINormal> & cloud, const Transform & transform = Transform(), bool filterNaNs = true);
cv::Mat RTABMAP_EXP laserScan2dFromPointCloud(const pcl::PointCloud<pcl::PointXYZI> & cloud, const pcl::PointCloud<pcl::Normal> & normals, const Transform & transform = Transform()); cv::Mat RTABMAP_EXP laserScan2dFromPointCloud(const pcl::PointCloud<pcl::PointXYZI> & cloud, const pcl::PointCloud<pcl::Normal> & normals, const Transform & transform = Transform(), bool filterNaNs = true);
// For 2d laserScan, z is set to null. // For 2d laserScan, z is set to null.
pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP laserScanToPointCloud(const LaserScan & laserScan, const Transform & transform = Transform()); pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP laserScanToPointCloud(const LaserScan & laserScan, const Transform & transform = Transform());
corelib/src/DBDriverSqlite3.cpp
+5
View File
@@ -4328,8 +4328,10 @@ void DBDriverSqlite3::saveOptimizedMeshQuery(
} }
else else
{ {
UDEBUG("Cloud points=%d", cloud.cols);
compressedCloud = compressData2(cloud); compressedCloud = compressData2(cloud);
} }
UDEBUG("Cloud compressed bytes=%d", compressedCloud.cols);
rc = sqlite3_bind_blob(ppStmt, index++, compressedCloud.data, compressedCloud.cols, SQLITE_STATIC); rc = sqlite3_bind_blob(ppStmt, index++, compressedCloud.data, compressedCloud.cols, SQLITE_STATIC);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str()); UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
@@ -4392,6 +4394,7 @@ void DBDriverSqlite3::saveOptimizedMeshQuery(
UASSERT(texCoords.empty() || polygons.size() == texCoords.size()); UASSERT(texCoords.empty() || polygons.size() == texCoords.size());
for(unsigned int t=0; t<polygons.size(); ++t) for(unsigned int t=0; t<polygons.size(); ++t)
{ {
UDEBUG("t=%d, polygons=%d", t, (int)polygons[t].size());
unsigned int materialPolygonIndices = 0; unsigned int materialPolygonIndices = 0;
for(unsigned int p=0; p<polygons[t].size(); ++p) for(unsigned int p=0; p<polygons[t].size(); ++p)
{ {
@@ -4446,6 +4449,7 @@ void DBDriverSqlite3::saveOptimizedMeshQuery(
} }
} }
UDEBUG("serializedPolygons=%d", (int)serializedPolygons.size());
compressedPolygons = compressData2(cv::Mat(1,serializedPolygons.size(), CV_32SC1, serializedPolygons.data())); compressedPolygons = compressData2(cv::Mat(1,serializedPolygons.size(), CV_32SC1, serializedPolygons.data()));
// polygon size // polygon size
@@ -4467,6 +4471,7 @@ void DBDriverSqlite3::saveOptimizedMeshQuery(
} }
else else
{ {
UDEBUG("serializedTexCoords=%d", (int)serializedTexCoords.size());
compressedTexCoords = compressData2(cv::Mat(1,serializedTexCoords.size(), CV_32FC1, serializedTexCoords.data())); compressedTexCoords = compressData2(cv::Mat(1,serializedTexCoords.size(), CV_32FC1, serializedTexCoords.data()));
rc = sqlite3_bind_blob(ppStmt, index++, compressedTexCoords.data, compressedTexCoords.cols, SQLITE_STATIC); rc = sqlite3_bind_blob(ppStmt, index++, compressedTexCoords.data, compressedTexCoords.cols, SQLITE_STATIC);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str()); UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
corelib/src/util3d.cpp
+54 -54
View File
@@ -1262,7 +1262,7 @@ pcl::PointCloud<pcl::PointXYZ> laserScanFromDepthImages(
return scan; return scan;
} }
LaserScan laserScanFromPointCloud(const pcl::PCLPointCloud2 & cloud) LaserScan laserScanFromPointCloud(const pcl::PCLPointCloud2 & cloud, bool filterNaNs)
{ {
if(cloud.data.empty()) if(cloud.data.empty())
{ {
@@ -1476,7 +1476,7 @@ LaserScan laserScanFromPointCloud(const pcl::PCLPointCloud2 & cloud)
UFATAL("Cannot handle as many channels (%d)!", laserScan.channels()); UFATAL("Cannot handle as many channels (%d)!", laserScan.channels());
} }
if(valid) if(!filterNaNs || valid)
{ {
++oi; ++oi;
} }
@@ -1489,11 +1489,11 @@ LaserScan laserScanFromPointCloud(const pcl::PCLPointCloud2 & cloud)
return LaserScan(laserScan(cv::Range::all(), cv::Range(0,oi)), 0, 0, format); return LaserScan(laserScan(cv::Range::all(), cv::Range(0,oi)), 0, 0, format);
} }
cv::Mat laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZ> & cloud, const Transform & transform) cv::Mat laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZ> & cloud, const Transform & transform, bool filterNaNs)
{ {
return laserScanFromPointCloud(cloud, pcl::IndicesPtr(), transform); return laserScanFromPointCloud(cloud, pcl::IndicesPtr(), transform, filterNaNs);
} }
cv::Mat laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZ> & cloud, const pcl::IndicesPtr & indices, const Transform & transform) cv::Mat laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZ> & cloud, const pcl::IndicesPtr & indices, const Transform & transform, bool filterNaNs)
{ {
cv::Mat laserScan; cv::Mat laserScan;
bool nullTransform = transform.isNull() || transform.isIdentity(); bool nullTransform = transform.isNull() || transform.isIdentity();
@@ -1505,7 +1505,7 @@ cv::Mat laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZ> & cloud, co
for(unsigned int i=0; i<indices->size(); ++i) for(unsigned int i=0; i<indices->size(); ++i)
{ {
int index = indices->at(i); int index = indices->at(i);
if(pcl::isFinite(cloud.at(index))) if(!filterNaNs || pcl::isFinite(cloud.at(index)))
{ {
float * ptr = laserScan.ptr<float>(0, oi++); float * ptr = laserScan.ptr<float>(0, oi++);
if(!nullTransform) if(!nullTransform)
@@ -1529,7 +1529,7 @@ cv::Mat laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZ> & cloud, co
laserScan = cv::Mat(1, (int)cloud.size(), CV_32FC3); laserScan = cv::Mat(1, (int)cloud.size(), CV_32FC3);
for(unsigned int i=0; i<cloud.size(); ++i) for(unsigned int i=0; i<cloud.size(); ++i)
{ {
if(pcl::isFinite(cloud.at(i))) if(!filterNaNs || pcl::isFinite(cloud.at(i)))
{ {
float * ptr = laserScan.ptr<float>(0, oi++); float * ptr = laserScan.ptr<float>(0, oi++);
if(!nullTransform) if(!nullTransform)
@@ -1555,11 +1555,11 @@ cv::Mat laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZ> & cloud, co
return laserScan(cv::Range::all(), cv::Range(0,oi)); return laserScan(cv::Range::all(), cv::Range(0,oi));
} }
cv::Mat laserScanFromPointCloud(const pcl::PointCloud<pcl::PointNormal> & cloud, const Transform & transform) cv::Mat laserScanFromPointCloud(const pcl::PointCloud<pcl::PointNormal> & cloud, const Transform & transform, bool filterNaNs)
{ {
return laserScanFromPointCloud(cloud, pcl::IndicesPtr(), transform); return laserScanFromPointCloud(cloud, pcl::IndicesPtr(), transform, filterNaNs);
} }
cv::Mat laserScanFromPointCloud(const pcl::PointCloud<pcl::PointNormal> & cloud, const pcl::IndicesPtr & indices, const Transform & transform) cv::Mat laserScanFromPointCloud(const pcl::PointCloud<pcl::PointNormal> & cloud, const pcl::IndicesPtr & indices, const Transform & transform, bool filterNaNs)
{ {
cv::Mat laserScan; cv::Mat laserScan;
bool nullTransform = transform.isNull() || transform.isIdentity(); bool nullTransform = transform.isNull() || transform.isIdentity();
@@ -1570,10 +1570,10 @@ cv::Mat laserScanFromPointCloud(const pcl::PointCloud<pcl::PointNormal> & cloud,
for(unsigned int i=0; i<indices->size(); ++i) for(unsigned int i=0; i<indices->size(); ++i)
{ {
int index = indices->at(i); int index = indices->at(i);
if(pcl::isFinite(cloud.at(index)) && if(!filterNaNs || (pcl::isFinite(cloud.at(index)) &&
uIsFinite(cloud.at(index).normal_x) && uIsFinite(cloud.at(index).normal_x) &&
uIsFinite(cloud.at(index).normal_y) && uIsFinite(cloud.at(index).normal_y) &&
uIsFinite(cloud.at(index).normal_z)) uIsFinite(cloud.at(index).normal_z)))
{ {
float * ptr = laserScan.ptr<float>(0, oi++); float * ptr = laserScan.ptr<float>(0, oi++);
if(!nullTransform) if(!nullTransform)
@@ -1603,10 +1603,10 @@ cv::Mat laserScanFromPointCloud(const pcl::PointCloud<pcl::PointNormal> & cloud,
laserScan = cv::Mat(1, (int)cloud.size(), CV_32FC(6)); laserScan = cv::Mat(1, (int)cloud.size(), CV_32FC(6));
for(unsigned int i=0; i<cloud.size(); ++i) for(unsigned int i=0; i<cloud.size(); ++i)
{ {
if(pcl::isFinite(cloud.at(i)) && if(!filterNaNs || (pcl::isFinite(cloud.at(i)) &&
uIsFinite(cloud.at(i).normal_x) && uIsFinite(cloud.at(i).normal_x) &&
uIsFinite(cloud.at(i).normal_y) && uIsFinite(cloud.at(i).normal_y) &&
uIsFinite(cloud.at(i).normal_z)) uIsFinite(cloud.at(i).normal_z)))
{ {
float * ptr = laserScan.ptr<float>(0, oi++); float * ptr = laserScan.ptr<float>(0, oi++);
if(!nullTransform) if(!nullTransform)
@@ -1638,7 +1638,7 @@ cv::Mat laserScanFromPointCloud(const pcl::PointCloud<pcl::PointNormal> & cloud,
return laserScan(cv::Range::all(), cv::Range(0,oi)); return laserScan(cv::Range::all(), cv::Range(0,oi));
} }
cv::Mat laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZ> & cloud, const pcl::PointCloud<pcl::Normal> & normals, const Transform & transform) cv::Mat laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZ> & cloud, const pcl::PointCloud<pcl::Normal> & normals, const Transform & transform, bool filterNaNs)
{ {
UASSERT(cloud.size() == normals.size()); UASSERT(cloud.size() == normals.size());
cv::Mat laserScan = cv::Mat(1, (int)cloud.size(), CV_32FC(6)); cv::Mat laserScan = cv::Mat(1, (int)cloud.size(), CV_32FC(6));
@@ -1646,7 +1646,7 @@ cv::Mat laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZ> & cloud, co
int oi =0; int oi =0;
for(unsigned int i=0; i<cloud.size(); ++i) for(unsigned int i=0; i<cloud.size(); ++i)
{ {
if(pcl::isFinite(cloud.at(i)) && pcl::isFinite(normals.at(i))) if(!filterNaNs || (pcl::isFinite(cloud.at(i)) && pcl::isFinite(normals.at(i))))
{ {
float * ptr = laserScan.ptr<float>(0, oi++); float * ptr = laserScan.ptr<float>(0, oi++);
if(!nullTransform) if(!nullTransform)
@@ -1684,12 +1684,12 @@ cv::Mat laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZ> & cloud, co
return laserScan(cv::Range::all(), cv::Range(0,oi)); return laserScan(cv::Range::all(), cv::Range(0,oi));
} }
cv::Mat laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZRGB> & cloud, const Transform & transform) cv::Mat laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZRGB> & cloud, const Transform & transform, bool filterNaNs)
{ {
return laserScanFromPointCloud(cloud, pcl::IndicesPtr(), transform); return laserScanFromPointCloud(cloud, pcl::IndicesPtr(), transform, filterNaNs);
} }
cv::Mat laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZRGB> & cloud, const pcl::IndicesPtr & indices, const Transform & transform) cv::Mat laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZRGB> & cloud, const pcl::IndicesPtr & indices, const Transform & transform, bool filterNaNs)
{ {
cv::Mat laserScan; cv::Mat laserScan;
bool nullTransform = transform.isNull() || transform.isIdentity(); bool nullTransform = transform.isNull() || transform.isIdentity();
@@ -1701,7 +1701,7 @@ cv::Mat laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZRGB> & cloud,
for(unsigned int i=0; i<indices->size(); ++i) for(unsigned int i=0; i<indices->size(); ++i)
{ {
int index = indices->at(i); int index = indices->at(i);
if(pcl::isFinite(cloud.at(index))) if(!filterNaNs || pcl::isFinite(cloud.at(index)))
{ {
float * ptr = laserScan.ptr<float>(0, oi++); float * ptr = laserScan.ptr<float>(0, oi++);
if(!nullTransform) if(!nullTransform)
@@ -1727,7 +1727,7 @@ cv::Mat laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZRGB> & cloud,
laserScan = cv::Mat(1, (int)cloud.size(), CV_32FC(4)); laserScan = cv::Mat(1, (int)cloud.size(), CV_32FC(4));
for(unsigned int i=0; i<cloud.size(); ++i) for(unsigned int i=0; i<cloud.size(); ++i)
{ {
if(pcl::isFinite(cloud.at(i))) if(!filterNaNs || pcl::isFinite(cloud.at(i)))
{ {
float * ptr = laserScan.ptr<float>(0, oi++); float * ptr = laserScan.ptr<float>(0, oi++);
if(!nullTransform) if(!nullTransform)
@@ -1755,12 +1755,12 @@ cv::Mat laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZRGB> & cloud,
return laserScan(cv::Range::all(), cv::Range(0,oi)); return laserScan(cv::Range::all(), cv::Range(0,oi));
} }
cv::Mat laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZI> & cloud, const Transform & transform) cv::Mat laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZI> & cloud, const Transform & transform, bool filterNaNs)
{ {
return laserScanFromPointCloud(cloud, pcl::IndicesPtr(), transform); return laserScanFromPointCloud(cloud, pcl::IndicesPtr(), transform, filterNaNs);
} }
cv::Mat laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZI> & cloud, const pcl::IndicesPtr & indices, const Transform & transform) cv::Mat laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZI> & cloud, const pcl::IndicesPtr & indices, const Transform & transform, bool filterNaNs)
{ {
cv::Mat laserScan; cv::Mat laserScan;
bool nullTransform = transform.isNull() || transform.isIdentity(); bool nullTransform = transform.isNull() || transform.isIdentity();
@@ -1772,7 +1772,7 @@ cv::Mat laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZI> & cloud, c
for(unsigned int i=0; i<indices->size(); ++i) for(unsigned int i=0; i<indices->size(); ++i)
{ {
int index = indices->at(i); int index = indices->at(i);
if(pcl::isFinite(cloud.at(index))) if(!filterNaNs || pcl::isFinite(cloud.at(index)))
{ {
float * ptr = laserScan.ptr<float>(0, oi++); float * ptr = laserScan.ptr<float>(0, oi++);
if(!nullTransform) if(!nullTransform)
@@ -1797,7 +1797,7 @@ cv::Mat laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZI> & cloud, c
laserScan = cv::Mat(1, (int)cloud.size(), CV_32FC(4)); laserScan = cv::Mat(1, (int)cloud.size(), CV_32FC(4));
for(unsigned int i=0; i<cloud.size(); ++i) for(unsigned int i=0; i<cloud.size(); ++i)
{ {
if(pcl::isFinite(cloud.at(i))) if(!filterNaNs || pcl::isFinite(cloud.at(i)))
{ {
float * ptr = laserScan.ptr<float>(0, oi++); float * ptr = laserScan.ptr<float>(0, oi++);
if(!nullTransform) if(!nullTransform)
@@ -1824,7 +1824,7 @@ cv::Mat laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZI> & cloud, c
return laserScan(cv::Range::all(), cv::Range(0,oi)); return laserScan(cv::Range::all(), cv::Range(0,oi));
} }
cv::Mat laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZRGB> & cloud, const pcl::PointCloud<pcl::Normal> & normals, const Transform & transform) cv::Mat laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZRGB> & cloud, const pcl::PointCloud<pcl::Normal> & normals, const Transform & transform, bool filterNaNs)
{ {
UASSERT(cloud.size() == normals.size()); UASSERT(cloud.size() == normals.size());
cv::Mat laserScan(1, (int)cloud.size(), CV_32FC(7)); cv::Mat laserScan(1, (int)cloud.size(), CV_32FC(7));
@@ -1832,7 +1832,7 @@ cv::Mat laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZRGB> & cloud,
int oi = 0; int oi = 0;
for(unsigned int i=0; i<cloud.size(); ++i) for(unsigned int i=0; i<cloud.size(); ++i)
{ {
if(pcl::isFinite(cloud.at(i))) if(!filterNaNs || pcl::isFinite(cloud.at(i)))
{ {
float * ptr = laserScan.ptr<float>(0, oi++); float * ptr = laserScan.ptr<float>(0, oi++);
if(!nullTransform) if(!nullTransform)
@@ -1872,11 +1872,11 @@ cv::Mat laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZRGB> & cloud,
return laserScan(cv::Range::all(), cv::Range(0,oi)); return laserScan(cv::Range::all(), cv::Range(0,oi));
} }
cv::Mat laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZRGBNormal> & cloud, const Transform & transform) cv::Mat laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZRGBNormal> & cloud, const Transform & transform, bool filterNaNs)
{ {
return laserScanFromPointCloud(cloud, pcl::IndicesPtr(), transform); return laserScanFromPointCloud(cloud, pcl::IndicesPtr(), transform, filterNaNs);
} }
cv::Mat laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZRGBNormal> & cloud, const pcl::IndicesPtr & indices, const Transform & transform) cv::Mat laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZRGBNormal> & cloud, const pcl::IndicesPtr & indices, const Transform & transform, bool filterNaNs)
{ {
cv::Mat laserScan; cv::Mat laserScan;
bool nullTransform = transform.isNull() || transform.isIdentity(); bool nullTransform = transform.isNull() || transform.isIdentity();
@@ -1887,10 +1887,10 @@ cv::Mat laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZRGBNormal> &
for(unsigned int i=0; i<indices->size(); ++i) for(unsigned int i=0; i<indices->size(); ++i)
{ {
int index = indices->at(i); int index = indices->at(i);
if(pcl::isFinite(cloud.at(index)) && if(!filterNaNs || (pcl::isFinite(cloud.at(index)) &&
uIsFinite(cloud.at(index).normal_x) && uIsFinite(cloud.at(index).normal_x) &&
uIsFinite(cloud.at(index).normal_y) && uIsFinite(cloud.at(index).normal_y) &&
uIsFinite(cloud.at(index).normal_z)) uIsFinite(cloud.at(index).normal_z)))
{ {
float * ptr = laserScan.ptr<float>(0, oi++); float * ptr = laserScan.ptr<float>(0, oi++);
if(!nullTransform) if(!nullTransform)
@@ -1922,10 +1922,10 @@ cv::Mat laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZRGBNormal> &
laserScan = cv::Mat(1, (int)cloud.size(), CV_32FC(7)); laserScan = cv::Mat(1, (int)cloud.size(), CV_32FC(7));
for(unsigned int i=0; i<cloud.size(); ++i) for(unsigned int i=0; i<cloud.size(); ++i)
{ {
if(pcl::isFinite(cloud.at(i)) && if(!filterNaNs || (pcl::isFinite(cloud.at(i)) &&
uIsFinite(cloud.at(i).normal_x) && uIsFinite(cloud.at(i).normal_x) &&
uIsFinite(cloud.at(i).normal_y) && uIsFinite(cloud.at(i).normal_y) &&
uIsFinite(cloud.at(i).normal_z)) uIsFinite(cloud.at(i).normal_z)))
{ {
float * ptr = laserScan.ptr<float>(0, oi++); float * ptr = laserScan.ptr<float>(0, oi++);
if(!nullTransform) if(!nullTransform)
@@ -1959,7 +1959,7 @@ cv::Mat laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZRGBNormal> &
return laserScan(cv::Range::all(), cv::Range(0,oi)); return laserScan(cv::Range::all(), cv::Range(0,oi));
} }
cv::Mat laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZI> & cloud, const pcl::PointCloud<pcl::Normal> & normals, const Transform & transform) cv::Mat laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZI> & cloud, const pcl::PointCloud<pcl::Normal> & normals, const Transform & transform, bool filterNaNs)
{ {
UASSERT(cloud.size() == normals.size()); UASSERT(cloud.size() == normals.size());
cv::Mat laserScan(1, (int)cloud.size(), CV_32FC(7)); cv::Mat laserScan(1, (int)cloud.size(), CV_32FC(7));
@@ -1967,7 +1967,7 @@ cv::Mat laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZI> & cloud, c
int oi=0; int oi=0;
for(unsigned int i=0; i<cloud.size(); ++i) for(unsigned int i=0; i<cloud.size(); ++i)
{ {
if(pcl::isFinite(cloud.at(i)) && pcl::isFinite(normals.at(i))) if(!filterNaNs || (pcl::isFinite(cloud.at(i)) && pcl::isFinite(normals.at(i))))
{ {
float * ptr = laserScan.ptr<float>(0, oi++); float * ptr = laserScan.ptr<float>(0, oi++);
if(!nullTransform) if(!nullTransform)
@@ -2005,17 +2005,17 @@ cv::Mat laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZI> & cloud, c
} }
return laserScan(cv::Range::all(), cv::Range(0,oi)); return laserScan(cv::Range::all(), cv::Range(0,oi));
} }
cv::Mat laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZINormal> & cloud, const Transform & transform) cv::Mat laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZINormal> & cloud, const Transform & transform, bool filterNaNs)
{ {
cv::Mat laserScan(1, (int)cloud.size(), CV_32FC(7)); cv::Mat laserScan(1, (int)cloud.size(), CV_32FC(7));
bool nullTransform = transform.isNull() || transform.isIdentity(); bool nullTransform = transform.isNull() || transform.isIdentity();
int oi = 0; int oi = 0;
for(unsigned int i=0; i<cloud.size(); ++i) for(unsigned int i=0; i<cloud.size(); ++i)
{ {
if(pcl::isFinite(cloud.at(i)) && if(!filterNaNs || (pcl::isFinite(cloud.at(i)) &&
uIsFinite(cloud.at(i).normal_x) && uIsFinite(cloud.at(i).normal_x) &&
uIsFinite(cloud.at(i).normal_y) && uIsFinite(cloud.at(i).normal_y) &&
uIsFinite(cloud.at(i).normal_z)) uIsFinite(cloud.at(i).normal_z)))
{ {
float * ptr = laserScan.ptr<float>(0, oi++); float * ptr = laserScan.ptr<float>(0, oi++);
if(!nullTransform) if(!nullTransform)
@@ -2047,7 +2047,7 @@ cv::Mat laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZINormal> & cl
return laserScan(cv::Range::all(), cv::Range(0,oi)); return laserScan(cv::Range::all(), cv::Range(0,oi));
} }
cv::Mat laserScan2dFromPointCloud(const pcl::PointCloud<pcl::PointXYZ> & cloud, const Transform & transform) cv::Mat laserScan2dFromPointCloud(const pcl::PointCloud<pcl::PointXYZ> & cloud, const Transform & transform, bool filterNaNs)
{ {
cv::Mat laserScan(1, (int)cloud.size(), CV_32FC2); cv::Mat laserScan(1, (int)cloud.size(), CV_32FC2);
bool nullTransform = transform.isNull(); bool nullTransform = transform.isNull();
@@ -2055,7 +2055,7 @@ cv::Mat laserScan2dFromPointCloud(const pcl::PointCloud<pcl::PointXYZ> & cloud,
int oi=0; int oi=0;
for(unsigned int i=0; i<cloud.size(); ++i) for(unsigned int i=0; i<cloud.size(); ++i)
{ {
if(pcl::isFinite(cloud.at(i))) if(!filterNaNs || pcl::isFinite(cloud.at(i)))
{ {
float * ptr = laserScan.ptr<float>(0, oi++); float * ptr = laserScan.ptr<float>(0, oi++);
if(!nullTransform) if(!nullTransform)
@@ -2079,7 +2079,7 @@ cv::Mat laserScan2dFromPointCloud(const pcl::PointCloud<pcl::PointXYZ> & cloud,
return laserScan(cv::Range::all(), cv::Range(0,oi)); return laserScan(cv::Range::all(), cv::Range(0,oi));
} }
cv::Mat laserScan2dFromPointCloud(const pcl::PointCloud<pcl::PointXYZI> & cloud, const Transform & transform) cv::Mat laserScan2dFromPointCloud(const pcl::PointCloud<pcl::PointXYZI> & cloud, const Transform & transform, bool filterNaNs)
{ {
cv::Mat laserScan(1, (int)cloud.size(), CV_32FC3); cv::Mat laserScan(1, (int)cloud.size(), CV_32FC3);
bool nullTransform = transform.isNull(); bool nullTransform = transform.isNull();
@@ -2087,7 +2087,7 @@ cv::Mat laserScan2dFromPointCloud(const pcl::PointCloud<pcl::PointXYZI> & cloud,
int oi=0; int oi=0;
for(unsigned int i=0; i<cloud.size(); ++i) for(unsigned int i=0; i<cloud.size(); ++i)
{ {
if(pcl::isFinite(cloud.at(i))) if(!filterNaNs || pcl::isFinite(cloud.at(i)))
{ {
float * ptr = laserScan.ptr<float>(0, oi++); float * ptr = laserScan.ptr<float>(0, oi++);
if(!nullTransform) if(!nullTransform)
@@ -2113,17 +2113,17 @@ cv::Mat laserScan2dFromPointCloud(const pcl::PointCloud<pcl::PointXYZI> & cloud,
return laserScan(cv::Range::all(), cv::Range(0,oi)); return laserScan(cv::Range::all(), cv::Range(0,oi));
} }
cv::Mat laserScan2dFromPointCloud(const pcl::PointCloud<pcl::PointNormal> & cloud, const Transform & transform) cv::Mat laserScan2dFromPointCloud(const pcl::PointCloud<pcl::PointNormal> & cloud, const Transform & transform, bool filterNaNs)
{ {
cv::Mat laserScan(1, (int)cloud.size(), CV_32FC(5)); cv::Mat laserScan(1, (int)cloud.size(), CV_32FC(5));
bool nullTransform = transform.isNull(); bool nullTransform = transform.isNull();
int oi=0; int oi=0;
for(unsigned int i=0; i<cloud.size(); ++i) for(unsigned int i=0; i<cloud.size(); ++i)
{ {
if(pcl::isFinite(cloud.at(i)) && if(!filterNaNs || (pcl::isFinite(cloud.at(i)) &&
uIsFinite(cloud.at(i).normal_x) && uIsFinite(cloud.at(i).normal_x) &&
uIsFinite(cloud.at(i).normal_y) && uIsFinite(cloud.at(i).normal_y) &&
uIsFinite(cloud.at(i).normal_z)) uIsFinite(cloud.at(i).normal_z)))
{ {
float * ptr = laserScan.ptr<float>(0, oi++); float * ptr = laserScan.ptr<float>(0, oi++);
if(!nullTransform) if(!nullTransform)
@@ -2153,7 +2153,7 @@ cv::Mat laserScan2dFromPointCloud(const pcl::PointCloud<pcl::PointNormal> & clou
return laserScan(cv::Range::all(), cv::Range(0,oi)); return laserScan(cv::Range::all(), cv::Range(0,oi));
} }
cv::Mat laserScan2dFromPointCloud(const pcl::PointCloud<pcl::PointXYZ> & cloud, const pcl::PointCloud<pcl::Normal> & normals, const Transform & transform) cv::Mat laserScan2dFromPointCloud(const pcl::PointCloud<pcl::PointXYZ> & cloud, const pcl::PointCloud<pcl::Normal> & normals, const Transform & transform, bool filterNaNs)
{ {
UASSERT(cloud.size() == normals.size()); UASSERT(cloud.size() == normals.size());
cv::Mat laserScan(1, (int)cloud.size(), CV_32FC(5)); cv::Mat laserScan(1, (int)cloud.size(), CV_32FC(5));
@@ -2161,7 +2161,7 @@ cv::Mat laserScan2dFromPointCloud(const pcl::PointCloud<pcl::PointXYZ> & cloud,
int oi=0; int oi=0;
for(unsigned int i=0; i<cloud.size(); ++i) for(unsigned int i=0; i<cloud.size(); ++i)
{ {
if(pcl::isFinite(cloud.at(i)) && pcl::isFinite(normals.at(i))) if(!filterNaNs || (pcl::isFinite(cloud.at(i)) && pcl::isFinite(normals.at(i))))
{ {
float * ptr = laserScan.ptr<float>(0, oi++); float * ptr = laserScan.ptr<float>(0, oi++);
if(!nullTransform) if(!nullTransform)
@@ -2197,17 +2197,17 @@ cv::Mat laserScan2dFromPointCloud(const pcl::PointCloud<pcl::PointXYZ> & cloud,
return laserScan(cv::Range::all(), cv::Range(0,oi)); return laserScan(cv::Range::all(), cv::Range(0,oi));
} }
cv::Mat laserScan2dFromPointCloud(const pcl::PointCloud<pcl::PointXYZINormal> & cloud, const Transform & transform) cv::Mat laserScan2dFromPointCloud(const pcl::PointCloud<pcl::PointXYZINormal> & cloud, const Transform & transform, bool filterNaNs)
{ {
cv::Mat laserScan(1, (int)cloud.size(), CV_32FC(6)); cv::Mat laserScan(1, (int)cloud.size(), CV_32FC(6));
bool nullTransform = transform.isNull(); bool nullTransform = transform.isNull();
int oi=0; int oi=0;
for(unsigned int i=0; i<cloud.size(); ++i) for(unsigned int i=0; i<cloud.size(); ++i)
{ {
if(pcl::isFinite(cloud.at(i)) && if(!filterNaNs || (pcl::isFinite(cloud.at(i)) &&
uIsFinite(cloud.at(i).normal_x) && uIsFinite(cloud.at(i).normal_x) &&
uIsFinite(cloud.at(i).normal_y) && uIsFinite(cloud.at(i).normal_y) &&
uIsFinite(cloud.at(i).normal_z)) uIsFinite(cloud.at(i).normal_z)))
{ {
float * ptr = laserScan.ptr<float>(0, oi++); float * ptr = laserScan.ptr<float>(0, oi++);
if(!nullTransform) if(!nullTransform)
@@ -2239,7 +2239,7 @@ cv::Mat laserScan2dFromPointCloud(const pcl::PointCloud<pcl::PointXYZINormal> &
return laserScan(cv::Range::all(), cv::Range(0,oi)); return laserScan(cv::Range::all(), cv::Range(0,oi));
} }
cv::Mat laserScan2dFromPointCloud(const pcl::PointCloud<pcl::PointXYZI> & cloud, const pcl::PointCloud<pcl::Normal> & normals, const Transform & transform) cv::Mat laserScan2dFromPointCloud(const pcl::PointCloud<pcl::PointXYZI> & cloud, const pcl::PointCloud<pcl::Normal> & normals, const Transform & transform, bool filterNaNs)
{ {
UASSERT(cloud.size() == normals.size()); UASSERT(cloud.size() == normals.size());
cv::Mat laserScan(1, (int)cloud.size(), CV_32FC(6)); cv::Mat laserScan(1, (int)cloud.size(), CV_32FC(6));
@@ -2247,7 +2247,7 @@ cv::Mat laserScan2dFromPointCloud(const pcl::PointCloud<pcl::PointXYZI> & cloud,
int oi=0; int oi=0;
for(unsigned int i=0; i<cloud.size(); ++i) for(unsigned int i=0; i<cloud.size(); ++i)
{ {
if(pcl::isFinite(cloud.at(i)) && pcl::isFinite(normals.at(i))) if(!filterNaNs || (pcl::isFinite(cloud.at(i)) && pcl::isFinite(normals.at(i))))
{ {
float * ptr = laserScan.ptr<float>(0, oi++); float * ptr = laserScan.ptr<float>(0, oi++);
if(!nullTransform) if(!nullTransform)