int main (int argc, char** argv) { // Setup points only typedef pcl::PointCloud<pcl::PointXYZ> CloudPointType; CloudPointType::Ptr cloudPoints(new CloudPointType); CloudPointType::PointType p; p.x = 1; p.y = 2; p.z = 3; cloudPoints->push_back(p); // Setup normals only typedef pcl::PointCloud<pcl::Normal> CloudNormalType; CloudNormalType::Ptr cloudNormals(new CloudNormalType); CloudNormalType::PointType n; n.normal_x = 4; n.normal_y = 5; n.normal_z = 6; cloudNormals->push_back(n); // Concatenate typedef pcl::PointCloud<pcl::PointNormal> CloudPointNormalType; CloudPointNormalType::Ptr cloudPointNormals(new CloudPointNormalType); pcl::concatenateFields (*cloudPoints, *cloudNormals, *cloudPointNormals); CloudPointNormalType::PointType p_retrieved = cloudPointNormals->points[0]; std::cout << p_retrieved.x << " " << p_retrieved.y << " " << p_retrieved.z << std::endl; std::cout << p_retrieved.normal_x << " " << p_retrieved.normal_y << " " << p_retrieved.normal_z << std::endl; return 0; }
int main(int argc, char** argv) { // Object for storing a pointcloud pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>); pcl::PointCloud<pcl::Normal>::Ptr normals(new pcl::PointCloud<pcl::Normal>); pcl::PointCloud<pcl::PointNormal>::Ptr cloudNormals(new pcl::PointCloud<pcl::PointNormal>); // Read a PCD file from disk if(pcl::io::loadPCDFile<pcl::PointXYZ>(argv[1],*cloud)!=0) { return -1; } // Normal estimation pcl::NormalEstimation<pcl::PointXYZ,pcl::Normal> normalEstimation; normalEstimation.setInputCloud(cloud); normalEstimation.setRadiusSearch(0.03); pcl::search::KdTree<pcl::PointXYZ>::Ptr kdtree(new pcl::search::KdTree<pcl::PointXYZ>); normalEstimation.setSearchMethod(kdtree); normalEstimation.compute(*normals); // The triangulation object requires the points and normals to be stored in the same structure pcl::concatenateFields(*cloud,*normals,*cloudNormals); // Tree object for searches in this new object pcl::search::KdTree<pcl::PointNormal>::Ptr kdtree2(new pcl::search::KdTree<pcl::PointNormal>); kdtree2->setInputCloud(cloudNormals); // Triangulation object pcl::GreedyProjectionTriangulation<pcl::PointNormal> triangulation; // Output object containing the mesh pcl::PolygonMesh triangles; // Maximum distance between connected points (maximum edge length) triangulation.setSearchRadius(30); // Maximum acceptable distance for a point to be considered, // relative to the distance of the nearest point triangulation.setMu(2.5); // Set the number of neighbors searched for triangulation.setMaximumNearestNeighbors(200); // Points will not be connected to the current points // if normals deviate more than the specified angle. triangulation.setMaximumSurfaceAngle(M_PI / 4); // 45 degrees // If false the direction of normals will not be taken into account // when computing the angle between them triangulation.setNormalConsistency(false); // Minimum and maximum angle there can be in a triangle // The first in not guaranteed, the second is triangulation.setMinimumAngle(M_PI / 18); // 10 degrees triangulation.setMaximumAngle(2*M_PI/ 3); // 120 degrees // Triangulate the cloud triangulation.setInputCloud(cloudNormals); triangulation.setSearchMethod(kdtree2); triangulation.reconstruct(triangles); // Save to disk pcl::io::saveVTKFile("mesh.vtk",triangles); return 0; }
void ObjectDescription::extractFPFHDescriptors(const pcl::PointCloud<pcl::PointXYZRGB>::Ptr& cloud) { pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudPtr (new pcl::PointCloud<pcl::PointXYZRGB>); pcl::PointCloud<pcl::PointXYZRGB>::Ptr sampledCloudPtr (new pcl::PointCloud<pcl::PointXYZRGB>); pcl::PointCloud<pcl::PointXYZRGB> objectPtCloudSampled; *cloudPtr = *cloud; pcl::VoxelGrid<pcl::PointXYZRGB> grid; grid.setInputCloud (cloudPtr); grid.setLeafSize (0.002, 0.002, 0.002); grid.filter (objectPtCloudSampled); *sampledCloudPtr = objectPtCloudSampled; pcl::NormalEstimation<pcl::PointXYZRGB, pcl::Normal> ne; ne.setInputCloud (sampledCloudPtr); pcl::search::KdTree<pcl::PointXYZRGB>::Ptr normalsTree (new pcl::search::KdTree<pcl::PointXYZRGB> ()); ne.setSearchMethod (normalsTree); pcl::PointCloud<pcl::Normal>::Ptr cloudNormals (new pcl::PointCloud<pcl::Normal>); ne.setRadiusSearch (0.03); ne.compute (*cloudNormals); pcl::FPFHEstimation<pcl::PointXYZRGB, pcl::Normal, pcl::FPFHSignature33> fpfh; fpfh.setInputCloud (sampledCloudPtr); fpfh.setInputNormals (cloudNormals); pcl::search::KdTree<pcl::PointXYZRGB>::Ptr fpfhTree (new pcl::search::KdTree<pcl::PointXYZRGB> ()); fpfh.setSearchMethod (fpfhTree); pcl::PointCloud<pcl::FPFHSignature33>::Ptr ptFeatHistograms (new pcl::PointCloud<pcl::FPFHSignature33> ()); fpfh.setRadiusSearch (0.05); fpfh.compute (*ptFeatHistograms); fpfhDescriptors = cv::Mat(ptFeatHistograms->size(), FPFH_LEN, CV_32F); for (size_t i = 0; i < ptFeatHistograms->size(); i++) { pcl::FPFHSignature33 feat = ptFeatHistograms->points[i]; float sum = 0.0; for(int j = 0; j < FPFH_LEN; j++ ) { fpfhDescriptors.at<float>(i, j) = feat.histogram[j]; sum += feat.histogram[j]; } for(int j = 0; j < FPFH_LEN; j++ ) { fpfhDescriptors.at<float>(i, j) /= sum; } } }
void CameraThread::postUpdate(SensorData * dataPtr, CameraInfo * info) const { UASSERT(dataPtr!=0); SensorData & data = *dataPtr; if(_colorOnly && !data.depthRaw().empty()) { data.setDepthOrRightRaw(cv::Mat()); } if(_distortionModel && !data.depthRaw().empty()) { UTimer timer; if(_distortionModel->getWidth() == data.depthRaw().cols && _distortionModel->getHeight() == data.depthRaw().rows ) { cv::Mat depth = data.depthRaw().clone();// make sure we are not modifying data in cached signatures. _distortionModel->undistort(depth); data.setDepthOrRightRaw(depth); } else { UERROR("Distortion model size is %dx%d but dpeth image is %dx%d!", _distortionModel->getWidth(), _distortionModel->getHeight(), data.depthRaw().cols, data.depthRaw().rows); } if(info) info->timeUndistortDepth = timer.ticks(); } if(_bilateralFiltering && !data.depthRaw().empty()) { UTimer timer; data.setDepthOrRightRaw(util2d::fastBilateralFiltering(data.depthRaw(), _bilateralSigmaS, _bilateralSigmaR)); if(info) info->timeBilateralFiltering = timer.ticks(); } if(_imageDecimation>1 && !data.imageRaw().empty()) { UDEBUG(""); UTimer timer; if(!data.depthRaw().empty() && !(data.depthRaw().rows % _imageDecimation == 0 && data.depthRaw().cols % _imageDecimation == 0)) { UERROR("Decimation of depth images should be exact (decimation=%d, size=(%d,%d))! " "Images won't be resized.", _imageDecimation, data.depthRaw().cols, data.depthRaw().rows); } else { data.setImageRaw(util2d::decimate(data.imageRaw(), _imageDecimation)); data.setDepthOrRightRaw(util2d::decimate(data.depthOrRightRaw(), _imageDecimation)); std::vector<CameraModel> models = data.cameraModels(); for(unsigned int i=0; i<models.size(); ++i) { if(models[i].isValidForProjection()) { models[i] = models[i].scaled(1.0/double(_imageDecimation)); } } data.setCameraModels(models); StereoCameraModel stereoModel = data.stereoCameraModel(); if(stereoModel.isValidForProjection()) { stereoModel.scale(1.0/double(_imageDecimation)); data.setStereoCameraModel(stereoModel); } } if(info) info->timeImageDecimation = timer.ticks(); } if(_mirroring && !data.imageRaw().empty() && data.cameraModels().size() == 1) { UDEBUG(""); UTimer timer; cv::Mat tmpRgb; cv::flip(data.imageRaw(), tmpRgb, 1); data.setImageRaw(tmpRgb); UASSERT_MSG(data.cameraModels().size() <= 1 && !data.stereoCameraModel().isValidForProjection(), "Only single RGBD cameras are supported for mirroring."); if(data.cameraModels().size() && data.cameraModels()[0].cx()) { CameraModel tmpModel( data.cameraModels()[0].fx(), data.cameraModels()[0].fy(), float(data.imageRaw().cols) - data.cameraModels()[0].cx(), data.cameraModels()[0].cy(), data.cameraModels()[0].localTransform()); data.setCameraModel(tmpModel); } if(!data.depthRaw().empty()) { cv::Mat tmpDepth; cv::flip(data.depthRaw(), tmpDepth, 1); data.setDepthOrRightRaw(tmpDepth); } if(info) info->timeMirroring = timer.ticks(); } if(_stereoToDepth && !data.imageRaw().empty() && data.stereoCameraModel().isValidForProjection() && !data.rightRaw().empty()) { UDEBUG(""); UTimer timer; cv::Mat depth = util2d::depthFromDisparity( _stereoDense->computeDisparity(data.imageRaw(), data.rightRaw()), data.stereoCameraModel().left().fx(), data.stereoCameraModel().baseline()); // set Tx for stereo bundle adjustment (when used) CameraModel model = CameraModel( data.stereoCameraModel().left().fx(), data.stereoCameraModel().left().fy(), data.stereoCameraModel().left().cx(), data.stereoCameraModel().left().cy(), data.stereoCameraModel().localTransform(), -data.stereoCameraModel().baseline()*data.stereoCameraModel().left().fx()); data.setCameraModel(model); data.setDepthOrRightRaw(depth); data.setStereoCameraModel(StereoCameraModel()); if(info) info->timeDisparity = timer.ticks(); } if(_scanFromDepth && data.cameraModels().size() && data.cameraModels().at(0).isValidForProjection() && !data.depthRaw().empty()) { UDEBUG(""); if(data.laserScanRaw().empty()) { UASSERT(_scanDecimation >= 1); UTimer timer; pcl::IndicesPtr validIndices(new std::vector<int>); pcl::PointCloud<pcl::PointXYZ>::Ptr cloud = util3d::cloudFromSensorData( data, _scanDecimation, _scanMaxDepth, _scanMinDepth, validIndices.get()); float maxPoints = (data.depthRaw().rows/_scanDecimation)*(data.depthRaw().cols/_scanDecimation); cv::Mat scan; const Transform & baseToScan = data.cameraModels()[0].localTransform(); if(validIndices->size()) { if(_scanVoxelSize>0.0f) { cloud = util3d::voxelize(cloud, validIndices, _scanVoxelSize); float ratio = float(cloud->size()) / float(validIndices->size()); maxPoints = ratio * maxPoints; } else if(!cloud->is_dense) { pcl::PointCloud<pcl::PointXYZ>::Ptr denseCloud(new pcl::PointCloud<pcl::PointXYZ>); pcl::copyPointCloud(*cloud, *validIndices, *denseCloud); cloud = denseCloud; } if(cloud->size()) { if(_scanNormalsK>0) { Eigen::Vector3f viewPoint(baseToScan.x(), baseToScan.y(), baseToScan.z()); pcl::PointCloud<pcl::Normal>::Ptr normals = util3d::computeNormals(cloud, _scanNormalsK, viewPoint); pcl::PointCloud<pcl::PointNormal>::Ptr cloudNormals(new pcl::PointCloud<pcl::PointNormal>); pcl::concatenateFields(*cloud, *normals, *cloudNormals); scan = util3d::laserScanFromPointCloud(*cloudNormals, baseToScan.inverse()); } else { scan = util3d::laserScanFromPointCloud(*cloud, baseToScan.inverse()); } } } data.setLaserScanRaw(scan, LaserScanInfo((int)maxPoints, _scanMaxDepth, baseToScan)); if(info) info->timeScanFromDepth = timer.ticks(); } else { UWARN("Option to create laser scan from depth image is enabled, but " "there is already a laser scan in the captured sensor data. Scan from " "depth will not be created."); } } }