mirror of
https://github.com/introlab/rtabmap.git
synced 2026-09-09 21:10:19 +08:00
Export: added random sample filter, added proportional radius filter, refactored when normals are computed (now after the clouds are assembled and voxelized), moved ceiling and floor filtering inside assembling loop.
This commit is contained in:
matlabbe
+180
-38
@@ -109,6 +109,9 @@ void showUsage()
|
||||
" --voxel # Voxel size of the created clouds (default 0.01 m, 0 m with --scan).\n"
|
||||
" --noise_radius # Noise filtering search radius (default 0, 0=disabled).\n"
|
||||
" --noise_k # Noise filtering minimum neighbors in search radius (default 5, 0=disabled).\n"
|
||||
" --prop_radius_factor # Proportional radius filter factor (default 0, 0=disabled). Start tuning from 0.01.\n"
|
||||
" --prop_radius_scale # Proportional radius filter neighbor scale (default 1).\n"
|
||||
" --random_samples # Number of output samples using a random filter (default 0, 0=disabled).\n"
|
||||
" --color_radius # Radius used to colorize polygons (default 0.05 m, 0 m with --scan). Set 0 for nearest color.\n"
|
||||
" --scan Use laser scan for the point cloud.\n"
|
||||
" --save_in_db Save resulting assembled point cloud or mesh in the database.\n"
|
||||
@@ -164,6 +167,9 @@ int main(int argc, char * argv[])
|
||||
float voxelSize = -1.0f;
|
||||
float noiseRadius = 0.0f;
|
||||
int noiseMinNeighbors = 5;
|
||||
float proportionalRadiusFactor = 0.0f;
|
||||
float proportionalRadiusScale = 1.0f;
|
||||
int randomSamples = 0;
|
||||
int textureSize = 8192;
|
||||
int textureCount = 1;
|
||||
float textureRange = 0;
|
||||
@@ -602,6 +608,42 @@ int main(int argc, char * argv[])
|
||||
showUsage();
|
||||
}
|
||||
}
|
||||
else if(std::strcmp(argv[i], "--prop_radius_factor") == 0)
|
||||
{
|
||||
++i;
|
||||
if(i<argc-1)
|
||||
{
|
||||
proportionalRadiusFactor = uStr2Float(argv[i]);
|
||||
}
|
||||
else
|
||||
{
|
||||
showUsage();
|
||||
}
|
||||
}
|
||||
else if(std::strcmp(argv[i], "--prop_radius_scale") == 0)
|
||||
{
|
||||
++i;
|
||||
if(i<argc-1)
|
||||
{
|
||||
proportionalRadiusScale = uStr2Float(argv[i]);
|
||||
}
|
||||
else
|
||||
{
|
||||
showUsage();
|
||||
}
|
||||
}
|
||||
else if(std::strcmp(argv[i], "--random_samples") == 0)
|
||||
{
|
||||
++i;
|
||||
if(i<argc-1)
|
||||
{
|
||||
randomSamples = uStr2Int(argv[i]);
|
||||
}
|
||||
else
|
||||
{
|
||||
showUsage();
|
||||
}
|
||||
}
|
||||
else if(std::strcmp(argv[i], "--color_radius") == 0)
|
||||
{
|
||||
++i;
|
||||
@@ -893,8 +935,8 @@ int main(int argc, char * argv[])
|
||||
|
||||
// Construct the cloud
|
||||
printf("Create and assemble the clouds...\n");
|
||||
pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr mergedClouds(new pcl::PointCloud<pcl::PointXYZRGBNormal>);
|
||||
pcl::PointCloud<pcl::PointXYZINormal>::Ptr mergedCloudsI(new pcl::PointCloud<pcl::PointXYZINormal>);
|
||||
pcl::PointCloud<pcl::PointXYZRGB>::Ptr assembledCloud(new pcl::PointCloud<pcl::PointXYZRGB>);
|
||||
pcl::PointCloud<pcl::PointXYZI>::Ptr assembledCloudI(new pcl::PointCloud<pcl::PointXYZI>);
|
||||
std::map<int, rtabmap::Transform> robotPoses;
|
||||
std::vector<std::map<int, rtabmap::Transform> > cameraPoses;
|
||||
std::map<int, rtabmap::Transform> scanPoses;
|
||||
@@ -902,6 +944,8 @@ int main(int argc, char * argv[])
|
||||
std::map<int, std::vector<rtabmap::CameraModel> > cameraModels;
|
||||
std::map<int, cv::Mat> cameraDepths;
|
||||
int imagesExported = 0;
|
||||
std::vector<int> rawViewpointIndices;
|
||||
std::map<int, Transform> rawViewpoints;
|
||||
for(std::map<int, Transform>::iterator iter=optimizedPoses.lower_bound(1); iter!=optimizedPoses.end(); ++iter)
|
||||
{
|
||||
Signature node = nodes.find(iter->first)->second;
|
||||
@@ -1043,40 +1087,61 @@ int main(int argc, char * argv[])
|
||||
else if(cloudI.get() && !cloudI->empty())
|
||||
cloudI = rtabmap::util3d::transformPointCloud(cloudI, iter->second);
|
||||
|
||||
if(filter_ceiling != 0.0 || filter_floor != 0.0f)
|
||||
{
|
||||
if(cloud.get() && !cloud->empty())
|
||||
{
|
||||
cloud = util3d::passThrough(cloud, "z", filter_floor!=0.0f?filter_floor:(float)std::numeric_limits<int>::min(), filter_ceiling!=0.0f?filter_ceiling:(float)std::numeric_limits<int>::max());
|
||||
}
|
||||
if(cloudI.get() && !cloudI->empty())
|
||||
{
|
||||
cloudI = util3d::passThrough(cloudI, "z", filter_floor!=0.0f?filter_floor:(float)std::numeric_limits<int>::min(), filter_ceiling!=0.0f?filter_ceiling:(float)std::numeric_limits<int>::max());
|
||||
}
|
||||
}
|
||||
|
||||
Eigen::Vector3f viewpoint(iter->second.x(), iter->second.y(), iter->second.z());
|
||||
if(cloudFromScan)
|
||||
{
|
||||
Transform lidarViewpoint = iter->second * node.sensorData().laserScanRaw().localTransform();
|
||||
viewpoint = Eigen::Vector3f(iter->second.x(), iter->second.y(), iter->second.z());
|
||||
rawViewpoints.insert(std::make_pair(iter->first, lidarViewpoint));
|
||||
}
|
||||
else if(!node.sensorData().cameraModels().empty() && !node.sensorData().cameraModels()[0].localTransform().isNull())
|
||||
{
|
||||
Transform cameraViewpoint = iter->second * node.sensorData().cameraModels()[0].localTransform(); // take the first camera
|
||||
rawViewpoints.insert(std::make_pair(iter->first, cameraViewpoint));
|
||||
}
|
||||
else if(!node.sensorData().stereoCameraModel().localTransform().isNull())
|
||||
{
|
||||
Transform cameraViewpoint = iter->second * node.sensorData().stereoCameraModel().localTransform();
|
||||
rawViewpoints.insert(std::make_pair(iter->first, cameraViewpoint));
|
||||
}
|
||||
else
|
||||
{
|
||||
rawViewpoints.insert(*iter);
|
||||
}
|
||||
|
||||
if(cloud.get() && !cloud->empty())
|
||||
{
|
||||
pcl::PointCloud<pcl::Normal>::Ptr normals = rtabmap::util3d::computeNormals(cloud, 20, 0.0f, viewpoint);
|
||||
pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr cloudWithNormals(new pcl::PointCloud<pcl::PointXYZRGBNormal>);
|
||||
pcl::concatenateFields(*cloud, *normals, *cloudWithNormals);
|
||||
if(mergedClouds->size() == 0)
|
||||
if(assembledCloud->empty())
|
||||
{
|
||||
*mergedClouds = *cloudWithNormals;
|
||||
*assembledCloud = *cloud;
|
||||
}
|
||||
else
|
||||
{
|
||||
*mergedClouds += *cloudWithNormals;
|
||||
*assembledCloud += *cloud;
|
||||
}
|
||||
rawViewpointIndices.resize(assembledCloud->size(), iter->first);
|
||||
}
|
||||
else if(cloudI.get() && !cloudI->empty())
|
||||
{
|
||||
pcl::PointCloud<pcl::Normal>::Ptr normals = rtabmap::util3d::computeNormals(cloudI, 20, 0.0f, viewpoint);
|
||||
pcl::PointCloud<pcl::PointXYZINormal>::Ptr cloudIWithNormals(new pcl::PointCloud<pcl::PointXYZINormal>);
|
||||
pcl::concatenateFields(*cloudI, *normals, *cloudIWithNormals);
|
||||
if(mergedCloudsI->size() == 0)
|
||||
if(assembledCloudI->empty())
|
||||
{
|
||||
*mergedCloudsI = *cloudIWithNormals;
|
||||
*assembledCloudI = *cloudI;
|
||||
}
|
||||
else
|
||||
{
|
||||
*mergedCloudsI += *cloudIWithNormals;
|
||||
*assembledCloudI += *cloudI;
|
||||
}
|
||||
rawViewpointIndices.resize(assembledCloudI->size(), iter->first);
|
||||
}
|
||||
|
||||
if(models.empty() && node.sensorData().stereoCameraModel().isValidForProjection())
|
||||
@@ -1111,14 +1176,14 @@ int main(int argc, char * argv[])
|
||||
scanPoses.insert(std::make_pair(iter->first, iter->second*node.sensorData().laserScanCompressed().localTransform()));
|
||||
}
|
||||
}
|
||||
printf("Create and assemble the clouds... done (%fs, %d points).\n", timer.ticks(), !mergedClouds->empty()?(int)mergedClouds->size():(int)mergedCloudsI->size());
|
||||
printf("Create and assemble the clouds... done (%fs, %d points).\n", timer.ticks(), !assembledCloud->empty()?(int)assembledCloud->size():(int)assembledCloudI->size());
|
||||
|
||||
if(imagesExported>0)
|
||||
printf("%d images exported!\n", imagesExported);
|
||||
|
||||
ConsoleProgessState progressState;
|
||||
|
||||
if(!mergedClouds->empty() || !mergedCloudsI->empty())
|
||||
if(!assembledCloud->empty() || !assembledCloudI->empty())
|
||||
{
|
||||
if(saveInDb)
|
||||
{
|
||||
@@ -1160,35 +1225,112 @@ int main(int argc, char * argv[])
|
||||
}
|
||||
}
|
||||
|
||||
pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr cloudToExport = mergedClouds;
|
||||
pcl::PointCloud<pcl::PointXYZINormal>::Ptr cloudIToExport = mergedCloudsI;
|
||||
|
||||
if(filter_ceiling != 0.0 || filter_floor != 0.0f)
|
||||
if(proportionalRadiusFactor>0.0f)
|
||||
{
|
||||
printf("Passthrough filtering of the assembled cloud along z axis... (min=%f, max=%f, %d points)\n", filter_floor, filter_ceiling, !cloudToExport->empty()?(int)cloudToExport->size():(int)cloudIToExport->size());
|
||||
if(!cloudToExport->empty())
|
||||
printf("Proportional radius filtering of the assembled cloud... (factor=%f scale=%f, %d points)\n", proportionalRadiusFactor, proportionalRadiusScale, !assembledCloud->empty()?(int)assembledCloud->size():(int)assembledCloudI->size());
|
||||
pcl::IndicesPtr indices;
|
||||
if(!assembledCloud->empty())
|
||||
{
|
||||
cloudToExport = util3d::passThrough(cloudToExport, "z", filter_floor!=0.0f?filter_floor:(float)std::numeric_limits<int>::min(), filter_ceiling!=0.0f?filter_ceiling:(float)std::numeric_limits<int>::max());
|
||||
indices = util3d::proportionalRadiusFiltering(assembledCloud, rawViewpointIndices, rawViewpoints, proportionalRadiusFactor, proportionalRadiusScale);
|
||||
pcl::PointCloud<pcl::PointXYZRGB> tmp;
|
||||
pcl::copyPointCloud(*assembledCloud, *indices, tmp);
|
||||
*assembledCloud = tmp;
|
||||
}
|
||||
if(!cloudIToExport->empty())
|
||||
else if(!assembledCloudI->empty())
|
||||
{
|
||||
cloudIToExport = util3d::passThrough(cloudIToExport, "z", filter_floor!=0.0f?filter_floor:(float)std::numeric_limits<int>::min(), filter_ceiling!=0.0f?filter_ceiling:(float)std::numeric_limits<int>::max());
|
||||
indices = util3d::proportionalRadiusFiltering(assembledCloudI, rawViewpointIndices, rawViewpoints, proportionalRadiusFactor, proportionalRadiusScale);
|
||||
pcl::PointCloud<pcl::PointXYZI> tmp;
|
||||
pcl::copyPointCloud(*assembledCloudI, *indices, tmp);
|
||||
*assembledCloudI = tmp;
|
||||
}
|
||||
printf("Passthrough filtering of the assembled cloud alog z axis.... done! (%fs, %d points)\n", timer.ticks(), !cloudToExport->empty()?(int)cloudToExport->size():(int)cloudIToExport->size());
|
||||
if(indices.get())
|
||||
{
|
||||
std::vector<int> rawCameraIndicesTmp(indices->size());
|
||||
for (std::size_t i = 0; i < indices->size(); ++i)
|
||||
rawCameraIndicesTmp[i] = rawViewpointIndices[indices->at(i)];
|
||||
rawViewpointIndices = rawCameraIndicesTmp;
|
||||
}
|
||||
printf("Proportional radius filtering of the assembled cloud.... done! (%fs, %d points)\n", timer.ticks(), !assembledCloud->empty()?(int)assembledCloud->size():(int)assembledCloudI->size());
|
||||
}
|
||||
|
||||
pcl::PointCloud<pcl::PointXYZ>::Ptr rawAssembledCloud(new pcl::PointCloud<pcl::PointXYZ>);
|
||||
if(!assembledCloud->empty())
|
||||
pcl::copyPointCloud(*assembledCloud, *rawAssembledCloud); // used to adjust normal orientation
|
||||
else if(!assembledCloudI->empty())
|
||||
pcl::copyPointCloud(*assembledCloudI, *rawAssembledCloud); // used to adjust normal orientation
|
||||
|
||||
pcl::PointCloud<pcl::PointXYZ>::Ptr cloudWithoutNormals = rawAssembledCloud;
|
||||
|
||||
if(voxelSize>0.0f)
|
||||
{
|
||||
printf("Voxel grid filtering of the assembled cloud... (voxel=%f, %d points)\n", voxelSize, !cloudToExport->empty()?(int)cloudToExport->size():(int)cloudIToExport->size());
|
||||
printf("Voxel grid filtering of the assembled cloud... (voxel=%f, %d points)\n", voxelSize, !assembledCloud->empty()?(int)assembledCloud->size():(int)assembledCloudI->size());
|
||||
if(!assembledCloud->empty())
|
||||
{
|
||||
assembledCloud = util3d::voxelize(assembledCloud, voxelSize);
|
||||
cloudWithoutNormals.reset(new pcl::PointCloud<pcl::PointXYZ>);
|
||||
pcl::copyPointCloud(*assembledCloud, *cloudWithoutNormals);
|
||||
}
|
||||
else if(!assembledCloudI->empty())
|
||||
{
|
||||
assembledCloudI = util3d::voxelize(assembledCloudI, voxelSize);
|
||||
cloudWithoutNormals.reset(new pcl::PointCloud<pcl::PointXYZ>);
|
||||
pcl::copyPointCloud(*assembledCloudI, *cloudWithoutNormals);
|
||||
}
|
||||
printf("Voxel grid filtering of the assembled cloud.... done! (%fs, %d points)\n", timer.ticks(), !assembledCloud->empty()?(int)assembledCloud->size():(int)assembledCloudI->size());
|
||||
}
|
||||
|
||||
printf("Computing normals of the assembled cloud... (k=20, %d points)\n", !assembledCloud->empty()?(int)assembledCloud->size():(int)assembledCloudI->size());
|
||||
pcl::PointCloud<pcl::Normal>::Ptr normals = util3d::computeNormals(cloudWithoutNormals, 20, 0);
|
||||
|
||||
pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr cloudToExport(new pcl::PointCloud<pcl::PointXYZRGBNormal>);
|
||||
pcl::PointCloud<pcl::PointXYZINormal>::Ptr cloudIToExport(new pcl::PointCloud<pcl::PointXYZINormal>);
|
||||
if(!assembledCloud->empty())
|
||||
{
|
||||
UASSERT(assembledCloud->size() == normals->size());
|
||||
pcl::concatenateFields(*assembledCloud, *normals, *cloudToExport);
|
||||
printf("Computing normals of the assembled cloud... done! (%fs, %d points)\n", timer.ticks(), (int)assembledCloud->size());
|
||||
assembledCloud->clear();
|
||||
|
||||
// adjust with point of views
|
||||
printf("Adjust normals to viewpoints of the assembled cloud... (%d points)\n", (int)cloudToExport->size());
|
||||
util3d::adjustNormalsToViewPoints(
|
||||
rawViewpoints,
|
||||
rawAssembledCloud,
|
||||
rawViewpointIndices,
|
||||
cloudToExport);
|
||||
printf("Adjust normals to viewpoints of the assembled cloud... (%fs, %d points)\n", timer.ticks(), (int)cloudToExport->size());
|
||||
}
|
||||
else if(!assembledCloudI->empty())
|
||||
{
|
||||
UASSERT(assembledCloudI->size() == normals->size());
|
||||
pcl::concatenateFields(*assembledCloudI, *normals, *cloudIToExport);
|
||||
printf("Computing normals of the assembled cloud... done! (%fs, %d points)\n", timer.ticks(), (int)assembledCloudI->size());
|
||||
assembledCloudI->clear();
|
||||
|
||||
// adjust with point of views
|
||||
printf("Adjust normals to viewpoints of the assembled cloud... (%d points)\n", (int)cloudIToExport->size());
|
||||
util3d::adjustNormalsToViewPoints(
|
||||
rawViewpoints,
|
||||
rawAssembledCloud,
|
||||
rawViewpointIndices,
|
||||
cloudIToExport);
|
||||
printf("Adjust normals to viewpoints of the assembled cloud... (%fs, %d points)\n", timer.ticks(), (int)cloudIToExport->size());
|
||||
}
|
||||
cloudWithoutNormals->clear();
|
||||
rawAssembledCloud->clear();
|
||||
|
||||
if(randomSamples>0)
|
||||
{
|
||||
printf("Random samples filtering of the assembled cloud... (samples=%d, %d points)\n", randomSamples, !cloudToExport->empty()?(int)cloudToExport->size():(int)cloudIToExport->size());
|
||||
if(!cloudToExport->empty())
|
||||
{
|
||||
cloudToExport = util3d::voxelize(cloudToExport, voxelSize);
|
||||
cloudToExport = util3d::randomSampling(cloudToExport, randomSamples);
|
||||
}
|
||||
else if(!cloudIToExport->empty())
|
||||
{
|
||||
cloudIToExport = util3d::voxelize(cloudIToExport, voxelSize);
|
||||
cloudIToExport = util3d::randomSampling(cloudIToExport, randomSamples);
|
||||
}
|
||||
printf("Voxel grid filtering of the assembled cloud.... done! (%fs, %d points)\n", timer.ticks(), !cloudToExport->empty()?(int)cloudToExport->size():(int)cloudIToExport->size());
|
||||
printf("Random samples filtering of the assembled cloud.... done! (%fs, %d points)\n", timer.ticks(), !cloudToExport->empty()?(int)cloudToExport->size():(int)cloudIToExport->size());
|
||||
}
|
||||
|
||||
std::vector<int> pointToCamId;
|
||||
@@ -1449,14 +1591,14 @@ int main(int argc, char * argv[])
|
||||
// Meshing...
|
||||
if(mesh || texture)
|
||||
{
|
||||
if(!mergedCloudsI->empty())
|
||||
if(!cloudIToExport->empty())
|
||||
{
|
||||
pcl::copyPointCloud(*mergedCloudsI, *mergedClouds);
|
||||
mergedCloudsI->clear();
|
||||
pcl::copyPointCloud(*cloudIToExport, *cloudToExport);
|
||||
cloudIToExport->clear();
|
||||
}
|
||||
|
||||
Eigen::Vector4f min,max;
|
||||
pcl::getMinMax3D(*mergedClouds, min, max);
|
||||
pcl::getMinMax3D(*cloudToExport, min, max);
|
||||
float mapLength = uMax3(max[0]-min[0], max[1]-min[1], max[2]-min[2]);
|
||||
int optimizedDepth = 12;
|
||||
for(int i=6; i<12; ++i)
|
||||
@@ -1477,7 +1619,7 @@ int main(int argc, char * argv[])
|
||||
pcl::PolygonMesh::Ptr mesh(new pcl::PolygonMesh);
|
||||
pcl::Poisson<pcl::PointXYZRGBNormal> poisson;
|
||||
poisson.setDepth(optimizedDepth);
|
||||
poisson.setInputCloud(mergedClouds);
|
||||
poisson.setInputCloud(cloudToExport);
|
||||
poisson.reconstruct(*mesh);
|
||||
printf("Mesh reconstruction... done (%fs, %d polygons).\n", timer.ticks(), (int)mesh->polygons.size());
|
||||
|
||||
@@ -1491,7 +1633,7 @@ int main(int argc, char * argv[])
|
||||
mesh,
|
||||
0.0f,
|
||||
maxPolygons,
|
||||
mergedClouds,
|
||||
cloudToExport,
|
||||
colorRadius,
|
||||
!texture,
|
||||
doClean,
|
||||
|
||||
Reference in New Issue
Block a user