int main(int, char** argv) { std::string filename = argv[1]; std::cout << "Reading " << filename << std::endl; pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_xyz (new pcl::PointCloud<pcl::PointXYZ>); if(pcl::io::loadPCDFile<pcl::PointXYZ> (filename, *cloud_xyz) == -1) // load the file { PCL_ERROR ("Couldn't read file"); return -1; } std::cout << "points: " << cloud_xyz->points.size () <<std::endl; // Parameters for sift computation const float min_scale = 0.01f; const int n_octaves = 3; const int n_scales_per_octave = 4; const float min_contrast = 0.001f; // Estimate the normals of the cloud_xyz pcl::NormalEstimation<pcl::PointXYZ, pcl::PointNormal> ne; pcl::PointCloud<pcl::PointNormal>::Ptr cloud_normals (new pcl::PointCloud<pcl::PointNormal>); pcl::search::KdTree<pcl::PointXYZ>::Ptr tree_n(new pcl::search::KdTree<pcl::PointXYZ>()); ne.setInputCloud(cloud_xyz); ne.setSearchMethod(tree_n); ne.setRadiusSearch(0.2); ne.compute(*cloud_normals); // Copy the xyz info from cloud_xyz and add it to cloud_normals as the xyz field in PointNormals estimation is zero for(size_t i = 0; i<cloud_normals->points.size(); ++i) { cloud_normals->points[i].x = cloud_xyz->points[i].x; cloud_normals->points[i].y = cloud_xyz->points[i].y; cloud_normals->points[i].z = cloud_xyz->points[i].z; } // Estimate the sift interest points using normals values from xyz as the Intensity variants pcl::SIFTKeypoint<pcl::PointNormal, pcl::PointWithScale> sift; pcl::PointCloud<pcl::PointWithScale> result; pcl::search::KdTree<pcl::PointNormal>::Ptr tree(new pcl::search::KdTree<pcl::PointNormal> ()); sift.setSearchMethod(tree); sift.setScales(min_scale, n_octaves, n_scales_per_octave); sift.setMinimumContrast(min_contrast); sift.setInputCloud(cloud_normals); sift.compute(result); std::cout << "No of SIFT points in the result are " << result.points.size () << std::endl; /* // Copying the pointwithscale to pointxyz so as visualize the cloud pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_temp (new pcl::PointCloud<pcl::PointXYZ>); copyPointCloud(result, *cloud_temp); std::cout << "SIFT points in the cloud_temp are " << cloud_temp->points.size () << std::endl; // Visualization of keypoints along with the original cloud pcl::visualization::PCLVisualizer viewer("PCL Viewer"); pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> keypoints_color_handler (cloud_temp, 0, 255, 0); pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> cloud_color_handler (cloud_xyz, 255, 0, 0); viewer.setBackgroundColor( 0.0, 0.0, 0.0 ); viewer.addPointCloud(cloud_xyz, cloud_color_handler, "cloud"); viewer.addPointCloud(cloud_temp, keypoints_color_handler, "keypoints"); viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 7, "keypoints"); while(!viewer.wasStopped ()) { viewer.spinOnce (); } */ return 0; }
template <typename PointInT, typename PointOutT, typename PointRFT> void pcl::UniqueShapeContext<PointInT, PointOutT, PointRFT>::computePointDescriptor (size_t index, /*float rf[9],*/ std::vector<float> &desc) { pcl::Vector3fMapConst origin = input_->points[(*indices_)[index]].getVector3fMap (); const Eigen::Vector3f x_axis (frames_->points[index].x_axis[0], frames_->points[index].x_axis[1], frames_->points[index].x_axis[2]); //const Eigen::Vector3f& y_axis = frames_->points[index].y_axis.getNormalVector3fMap (); const Eigen::Vector3f normal (frames_->points[index].z_axis[0], frames_->points[index].z_axis[1], frames_->points[index].z_axis[2]); // Find every point within specified search_radius_ std::vector<int> nn_indices; std::vector<float> nn_dists; const size_t neighb_cnt = searchForNeighbors ((*indices_)[index], search_radius_, nn_indices, nn_dists); // For each point within radius for (size_t ne = 0; ne < neighb_cnt; ne++) { if (pcl::utils::equal(nn_dists[ne], 0.0f)) continue; // Get neighbours coordinates Eigen::Vector3f neighbour = surface_->points[nn_indices[ne]].getVector3fMap (); // ----- Compute current neighbour polar coordinates ----- // Get distance between the neighbour and the origin float r = std::sqrt (nn_dists[ne]); // Project point into the tangent plane Eigen::Vector3f proj; pcl::geometry::project (neighbour, origin, normal, proj); proj -= origin; // Normalize to compute the dot product proj.normalize (); // Compute the angle between the projection and the x axis in the interval [0,360] Eigen::Vector3f cross = x_axis.cross (proj); float phi = rad2deg (std::atan2 (cross.norm (), x_axis.dot (proj))); phi = cross.dot (normal) < 0.f ? (360.0f - phi) : phi; /// Compute the angle between the neighbour and the z axis (normal) in the interval [0, 180] Eigen::Vector3f no = neighbour - origin; no.normalize (); float theta = normal.dot (no); theta = pcl::rad2deg (acosf (std::min (1.0f, std::max (-1.0f, theta)))); /// Bin (j, k, l) size_t j = 0; size_t k = 0; size_t l = 0; /// Compute the Bin(j, k, l) coordinates of current neighbour for (size_t rad = 1; rad < radius_bins_ + 1; rad++) { if (r <= radii_interval_[rad]) { j = rad - 1; break; } } for (size_t ang = 1; ang < elevation_bins_ + 1; ang++) { if (theta <= theta_divisions_[ang]) { k = ang - 1; break; } } for (size_t ang = 1; ang < azimuth_bins_ + 1; ang++) { if (phi <= phi_divisions_[ang]) { l = ang - 1; break; } } /// Local point density = number of points in a sphere of radius "point_density_radius_" around the current neighbour std::vector<int> neighbour_indices; std::vector<float> neighbour_didtances; float point_density = static_cast<float> (searchForNeighbors (*surface_, nn_indices[ne], point_density_radius_, neighbour_indices, neighbour_didtances)); /// point_density is always bigger than 0 because FindPointsWithinRadius returns at least the point itself float w = (1.0f / point_density) * volume_lut_[(l*elevation_bins_*radius_bins_) + (k*radius_bins_) + j]; assert (w >= 0.0); if (w == std::numeric_limits<float>::infinity ()) PCL_ERROR ("Shape Context Error INF!\n"); if (std::isnan(w)) PCL_ERROR ("Shape Context Error IND!\n"); /// Accumulate w into correspondent Bin(j,k,l) desc[(l*elevation_bins_*radius_bins_) + (k*radius_bins_) + j] += w; assert (desc[(l*elevation_bins_*radius_bins_) + (k*radius_bins_) + j] >= 0); } // end for each neighbour }
template <typename PointT, typename PointNT> void pcl::SampleConsensusModelCylinder<PointT, PointNT>::projectPoints ( const std::vector<int> &inliers, const Eigen::VectorXf &model_coefficients, PointCloud &projected_points, bool copy_data_fields) { // Needs a valid set of model coefficients if (model_coefficients.size () != 7) { PCL_ERROR ("[pcl::SampleConsensusModelCylinder::projectPoints] Invalid number of model coefficients given (%zu)!\n", model_coefficients.size ()); return; } projected_points.header = input_->header; projected_points.is_dense = input_->is_dense; Eigen::Vector4f line_pt (model_coefficients[0], model_coefficients[1], model_coefficients[2], 0); Eigen::Vector4f line_dir (model_coefficients[3], model_coefficients[4], model_coefficients[5], 0); float ptdotdir = line_pt.dot (line_dir); float dirdotdir = 1.0f / line_dir.dot (line_dir); // Copy all the data fields from the input cloud to the projected one? if (copy_data_fields) { // Allocate enough space and copy the basics projected_points.points.resize (input_->points.size ()); projected_points.width = input_->width; projected_points.height = input_->height; typedef typename pcl::traits::fieldList<PointT>::type FieldList; // Iterate over each point for (size_t i = 0; i < projected_points.points.size (); ++i) // Iterate over each dimension pcl::for_each_type <FieldList> (NdConcatenateFunctor <PointT, PointT> (input_->points[i], projected_points.points[i])); // Iterate through the 3d points and calculate the distances from them to the cylinder for (size_t i = 0; i < inliers.size (); ++i) { Eigen::Vector4f p (input_->points[inliers[i]].x, input_->points[inliers[i]].y, input_->points[inliers[i]].z, 1); float k = (p.dot (line_dir) - ptdotdir) * dirdotdir; pcl::Vector4fMap pp = projected_points.points[inliers[i]].getVector4fMap (); pp.matrix () = line_pt + k * line_dir; Eigen::Vector4f dir = p - pp; dir.normalize (); // Calculate the projection of the point onto the cylinder pp += dir * model_coefficients[6]; } } else { // Allocate enough space and copy the basics projected_points.points.resize (inliers.size ()); projected_points.width = static_cast<uint32_t> (inliers.size ()); projected_points.height = 1; typedef typename pcl::traits::fieldList<PointT>::type FieldList; // Iterate over each point for (size_t i = 0; i < inliers.size (); ++i) // Iterate over each dimension pcl::for_each_type <FieldList> (NdConcatenateFunctor <PointT, PointT> (input_->points[inliers[i]], projected_points.points[i])); // Iterate through the 3d points and calculate the distances from them to the plane for (size_t i = 0; i < inliers.size (); ++i) { pcl::Vector4fMap pp = projected_points.points[i].getVector4fMap (); pcl::Vector4fMapConst p = input_->points[inliers[i]].getVector4fMap (); float k = (p.dot (line_dir) - ptdotdir) * dirdotdir; // Calculate the projection of the point on the line pp.matrix () = line_pt + k * line_dir; Eigen::Vector4f dir = p - pp; dir.normalize (); // Calculate the projection of the point onto the cylinder pp += dir * model_coefficients[6]; } } }
int main(int argc, char** argv) { if (argc < 5) { PCL_INFO ("Usage %s -input_dir /dir/with/models -output_dir /output/dir [options]\n", argv[0]); PCL_INFO (" * where options are:\n" " -height <X> : simulate scans with sensor mounted on height X\n" " -noise_std <X> : std of gaussian noise added to pointcloud. Default value 0.0001.\n" " -distance <X> : simulate scans with object located on a distance X. Can be used multiple times. Default value 4.\n" " -tilt <X> : tilt sensor for X degrees. X == 0 - sensor looks strait. X < 0 Sensor looks down. X > 0 Sensor looks up . Can be used multiple times. Default value -15.\n" " -shift <X> : shift object from the straight line. Can be used multiple times. Default value 0.\n" " -num_views <X> : how many times rotate the object in for every distance, tilt and shift. Default value 6.\n" ""); return -1; } std::string input_dir, output_dir; double height = 1.5; double num_views = 6; double noise_std = 0.0001; std::vector<double> distances; std::vector<double> tilt; std::vector<double> shift; int full_model_n_points = 30000; pcl::console::parse_argument(argc, argv, "-input_dir", input_dir); pcl::console::parse_argument(argc, argv, "-output_dir", output_dir); pcl::console::parse_argument(argc, argv, "-num_views", num_views); pcl::console::parse_argument(argc, argv, "-height", height); pcl::console::parse_argument(argc, argv, "-noise_std", noise_std); pcl::console::parse_multiple_arguments(argc, argv, "-distance", distances); pcl::console::parse_multiple_arguments(argc, argv, "-tilt", tilt); pcl::console::parse_multiple_arguments(argc, argv, "-shift", shift); PCL_INFO("distances size: %d\n", distances.size()); for (size_t i = 0; i < distances.size(); i++) { PCL_INFO("distance: %f\n", distances[i]); } // Set default values if user didn't provide any if (distances.size() == 0) distances.push_back(4); if (tilt.size() == 0) tilt.push_back(-15); if (shift.size() == 0) shift.push_back(0); // Check if input path exists boost::filesystem::path input_path(input_dir); if (!boost::filesystem::exists(input_path)) { PCL_ERROR("Input directory doesnt exists."); return -1; } // Check if input path is a directory if (!boost::filesystem::is_directory(input_path)) { PCL_ERROR("%s is not directory.", input_path.c_str()); return -1; } // Check if output directory exists boost::filesystem::path output_path(output_dir); if (!boost::filesystem::exists(output_path) || !boost::filesystem::is_directory(output_path)) { if (!boost::filesystem::create_directories(output_path)) { PCL_ERROR ("Error creating directory %s.\n", output_path.c_str ()); return -1; } } // Find all .vtk files in the input directory std::vector<std::string> files_to_process; PCL_INFO("Processing following files:\n"); boost::filesystem::directory_iterator end_iter; for (boost::filesystem::directory_iterator iter(input_path); iter != end_iter; iter++) { boost::filesystem::path class_dir_path(*iter); for (boost::filesystem::directory_iterator iter2(class_dir_path); iter2 != end_iter; iter2++) { boost::filesystem::path file(*iter2); if (file.extension() == ".vtk") { files_to_process.push_back(file.c_str()); PCL_INFO("\t%s\n", file.c_str()); } } } // Check if there are any .vtk files to process if (files_to_process.size() == 0) { PCL_ERROR("Directory %s has no .vtk files.", input_path.c_str()); return -1; } // Iterate over all files for (size_t i = 0; i < files_to_process.size(); i++) { vtkSmartPointer<vtkPolyData> model; vtkSmartPointer<vtkPolyDataReader> reader = vtkPolyDataReader::New(); vtkSmartPointer<vtkTransform> transform = vtkTransform::New(); pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>()); // Compute output directory for this model std::vector<std::string> st; boost::split(st, files_to_process.at(i), boost::is_any_of("/"), boost::token_compress_on); std::string class_dirname = st.at(st.size() - 2); std::string dirname = st.at(st.size() - 1); dirname = dirname.substr(0, dirname.size() - 4); dirname = output_dir + class_dirname + "/" + dirname; // Check if output directory for this model exists. If not create boost::filesystem::path dirpath(dirname); if (!boost::filesystem::exists(dirpath)) { if (!boost::filesystem::create_directories(dirpath)) { PCL_ERROR ("Error creating directory %s.\n", dirpath.c_str ()); return -1; } } // Load model from file reader->SetFileName(files_to_process.at(i).c_str()); reader->Update(); model = reader->GetOutput(); PCL_INFO("Number of points %d\n",model->GetNumberOfPoints()); // Coumpute bounds and center of the model double bounds[6]; model->GetBounds(bounds); double min_z_value = bounds[4]; double center[3]; model->GetCenter(center); createFullModelPointcloud(model, full_model_n_points, *cloud); pcl::io::savePCDFile(dirname + "/full.pcd", *cloud); // Initialize PCLVisualizer. Add model to scene. pcl::visualization::PCLVisualizer viz; viz.initCameraParameters(); viz.updateCamera(); viz.setCameraPosition(0, 0, 0, 1, 0, 0, 0, 0, 1); viz.addModelFromPolyData(model, transform); viz.setRepresentationToSurfaceForAllActors(); // Iterate over all shifts for (size_t shift_index = 0; shift_index < shift.size(); shift_index++) { // Iterate over all tilts for (size_t tilt_index = 0; tilt_index < tilt.size(); tilt_index++) { // Iterate over all distances for (size_t distance_index = 0; distance_index < distances.size(); distance_index++) { // Iterate over all angles double angle = 0; double angle_step = 360.0 / num_views; for (int angle_index = 0; angle_index < num_views; angle_index++) { // Set transformation with distance, shift, tilt and angle parameters. transform->Identity(); transform->RotateY(tilt[tilt_index]); transform->Translate(distances[distance_index], shift[shift_index], -(height + min_z_value)); transform->RotateZ(angle); /* // Render pointcloud viz.renderView(640, 480, cloud); //Add noise addNoise(cloud, noise_std); // Compute new coordinates of the model center double new_center[3]; transform->TransformPoint(center, new_center); // Shift origin of the poincloud to the model center and align with initial coordinate system. pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_transformed(new pcl::PointCloud<pcl::PointXYZ>()); moveToNewCenterAndAlign(cloud, cloud_transformed, new_center, tilt[tilt_index]); // Compute file name for this pointcloud and save it std::stringstream ss; ss << dirname << "/rotation" << angle << "_distance" << distances[distance_index] << "_tilt" << tilt[tilt_index] << "_shift" << shift[shift_index] << ".pcd"; pcl_INFO("Writing %d points to file %s\n", cloud->points.size(), ss.str().c_str()); pcl::io::savePCDFile(ss.str(), *cloud_transformed); */ // increment angle by step angle += angle_step; } } } } } return 0; }
int main (int argc, char *argv[]) { PointCloudT::Ptr cloud (new PointCloudT), cloud_inliers_table (new PointCloudT), cloud_outliers_table (new PointCloudT), cloud_edge (new PointCloudT), cloud_edge_projected (new PointCloudT), cloud_poligonal_prism (new PointCloudT); /*// Load point cloud if (pcl::io::loadPCDFile ("caixacomobjectos.pcd", *cloud) < 0) { PCL_ERROR ("Could not load PCD file !\n"); return (-1); }*/ // Load point cloud if (pcl::io::loadPCDFile (argv[1], *cloud) < 0) { PCL_ERROR ("Could not load PCD file !\n"); return (-1); } /////////////////////////////////////////// // Table Plane Extract // /////////////////////////////////////////// // Segment the ground pcl::ModelCoefficients::Ptr plane_table (new pcl::ModelCoefficients); pcl::PointIndices::Ptr inliers_plane_table (new pcl::PointIndices); PointCloudT::Ptr cloud_plane_table (new PointCloudT); // Make room for a plane equation (ax+by+cz+d=0) plane_table->values.resize (4); pcl::SACSegmentation<PointT> seg_table; // Create the segmentation object seg_table.setOptimizeCoefficients (true); // Optional seg_table.setMethodType (pcl::SAC_RANSAC); seg_table.setModelType (pcl::SACMODEL_PLANE); seg_table.setDistanceThreshold (0.025f); seg_table.setInputCloud (cloud); seg_table.segment (*inliers_plane_table, *plane_table); if (inliers_plane_table->indices.size () == 0) { PCL_ERROR ("Could not estimate a planar model for the given dataset.\n"); return (-1); } // Extract inliers pcl::ExtractIndices<PointT> extract; extract.setInputCloud (cloud); extract.setIndices (inliers_plane_table); extract.setNegative (false); // Extract the inliers extract.filter (*cloud_inliers_table); // cloud_inliers contains the plane // Extract outliers //extract.setInputCloud (cloud); // Already done line 50 //extract.setIndices (inliers); // Already done line 51 extract.setNegative (true); // Extract the outliers extract.filter (*cloud_outliers_table); // cloud_outliers contains everything but the plane printf ("Plane segmentation equation [ax+by+cz+d]=0: [%3.4f | %3.4f | %3.4f | %3.4f] \t\n", plane_table->values[0], plane_table->values[1], plane_table->values[2] , plane_table->values[3]); /////////////////////////////////////////// // Box Edge Extract // /////////////////////////////////////////// pcl::PassThrough<PointT> pass; pass.setInputCloud (cloud_outliers_table); pass.setFilterFieldName ("z"); pass.setFilterLimits (0.63, 0.68); pass.filter (*cloud_edge); pcl::ModelCoefficients::Ptr coefficients_edge (new pcl::ModelCoefficients); pcl::PointIndices::Ptr inliers_edge (new pcl::PointIndices); // Create the segmentation object pcl::SACSegmentation<pcl::PointXYZ> seg_edge; // Optional seg_edge.setOptimizeCoefficients (true); // Mandatory seg_edge.setModelType (pcl::SACMODEL_PLANE); seg_edge.setMethodType (pcl::SAC_RANSAC); seg_edge.setDistanceThreshold (0.01); seg_edge.setInputCloud (cloud_edge); seg_edge.segment (*inliers_edge, *coefficients_edge); /////////////////////////////// // Project the model inliers // /////////////////////////////// pcl::ProjectInliers<pcl::PointXYZ> proj_edge; proj_edge.setModelType (pcl::SACMODEL_PLANE); proj_edge.setIndices (inliers_edge); proj_edge.setInputCloud (cloud_edge); proj_edge.setModelCoefficients (coefficients_edge); proj_edge.filter (*cloud_edge_projected); double merge = 0.75; scale(cloud_edge_projected, merge); //////////////////////////////////////////////////////////////////// // Create a Concave Hull representation of the box edge // //////////////////////////////////////////////////////////////////// pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_edge_hull (new pcl::PointCloud<pcl::PointXYZ>); pcl::ConvexHull<pcl::PointXYZ> chull; chull.setInputCloud (cloud_edge_projected); //chull.setAlpha (0.1); chull.reconstruct (*cloud_edge_hull); /////////////////////////////////////////////// // Create a Poligonal Prism of the box edge // /////////////////////////////////////////////// pcl::PointIndices::Ptr inliers_poligonal_prism (new pcl::PointIndices); double z_min = -1.0, z_max = 0.0; // we want the points above the plane, no farther than 5 cm from the surface pcl::ExtractPolygonalPrismData<pcl::PointXYZ> prism; prism.setInputCloud (cloud_outliers_table); prism.setInputPlanarHull (cloud_edge_hull); prism.setHeightLimits (z_min, z_max); prism.segment (*inliers_poligonal_prism); // Extract poligonal inliers pcl::ExtractIndices<PointT> extract_polygonal_data; extract_polygonal_data.setInputCloud (cloud_outliers_table); extract_polygonal_data.setIndices (inliers_poligonal_prism); extract_polygonal_data.setNegative (false); // Extract the inliers extract_polygonal_data.filter (*cloud_poligonal_prism); // cloud_poligonal_prism contains the box //guarda a nuvem filtrada num novo ficheiro pcl::io::savePCDFileASCII ("test_seg_3pecas.pcd", *cloud_poligonal_prism); /////////////////// // Visualization // /////////////////// pcl::visualization::PCLVisualizer viewer ("PCL visualizer"); // Table Plane pcl::visualization::PointCloudColorHandlerCustom<PointT> cloud_inliers_table_handler (cloud, 20, 255, 255); viewer.addPointCloud (cloud_inliers_table, cloud_inliers_table_handler, "cloud inliers"); // Everything else in GRAY pcl::visualization::PointCloudColorHandlerCustom<PointT> cloud_outliers_table_handler (cloud, 200, 200, 200); viewer.addPointCloud (cloud_outliers_table, cloud_outliers_table_handler, "cloud outliers"); // Edge in Green pcl::visualization::PointCloudColorHandlerCustom<PointT> cloud_edge_handler (cloud, 20, 255, 20); viewer.addPointCloud (cloud_edge, cloud_edge_handler, "cloud edge"); // Edge projected pcl::visualization::PointCloudColorHandlerCustom<PointT> cloud_edge_projected_handler (cloud, 255, 69, 20); viewer.addPointCloud (cloud_edge_projected, cloud_edge_projected_handler, "cloud edge projected"); // Edge hull pcl::visualization::PointCloudColorHandlerCustom<PointT> cloud_edge_hull_handler (cloud, 255, 20, 20); viewer.addPointCloud (cloud_edge_hull, cloud_edge_hull_handler, "cloud edge hull"); // Poligonal Prism data pcl::visualization::PointCloudColorHandlerCustom<PointT> cloud_poligonal_prism_handler (cloud, 20, 20, 255); viewer.addPointCloud (cloud_poligonal_prism, cloud_poligonal_prism_handler, "cloud Poligonal Prism"); while (!viewer.wasStopped ()) { viewer.spinOnce (); } return (0); }
void pcl::kinfuLS::WorldModel<PointT>::getWorldAsCubes (const double size, std::vector<typename WorldModel<PointT>::PointCloudPtr> &cubes, std::vector<Eigen::Vector3f, Eigen::aligned_allocator<Eigen::Vector3f> > &transforms, double overlap) { if(world_->points.size () == 0) { PCL_INFO("The world is empty, returning nothing\n"); return; } PCL_INFO ("Getting world as cubes. World contains %d points.\n", world_->points.size ()); // remove nans from world cloud world_->is_dense = false; std::vector<int> indices; pcl::removeNaNFromPointCloud ( *world_, *world_, indices); PCL_INFO ("World contains %d points after nan removal.\n", world_->points.size ()); // check cube size value double cubeSide = size; if (cubeSide <= 0.0f) { PCL_ERROR ("Size of the cube must be positive and non null (%f given). Setting it to 3.0 meters.\n", cubeSide); cubeSide = 512.0f; } std::cout << "cube size is set to " << cubeSide << std::endl; // check overlap value double step_increment = 1.0f - overlap; if (overlap < 0.0) { PCL_ERROR ("Overlap ratio must be positive or null (%f given). Setting it to 0.0 procent.\n", overlap); step_increment = 1.0f; } if (overlap > 1.0) { PCL_ERROR ("Overlap ratio must be less or equal to 1.0 (%f given). Setting it to 10 procent.\n", overlap); step_increment = 0.1f; } // get world's bounding values on XYZ PointT min, max; pcl::getMinMax3D(*world_, min, max); PCL_INFO ("Bounding box for the world: \n\t [%f - %f] \n\t [%f - %f] \n\t [%f - %f] \n", min.x, max.x, min.y, max.y, min.z, max.z); PointT origin = min; // clear returned vectors cubes.clear(); transforms.clear(); // iterate with box filter while (origin.x < max.x) { origin.y = min.y; while (origin.y < max.y) { origin.z = min.z; while (origin.z < max.z) { // extract cube here PCL_INFO ("Extracting cube at: [%f, %f, %f].\n", origin.x, origin.y, origin.z); // pointcloud for current cube. PointCloudPtr box (new pcl::PointCloud<PointT>); // set conditional filter ConditionAndPtr range_cond (new pcl::ConditionAnd<PointT> ()); range_cond->addComparison (FieldComparisonConstPtr (new pcl::FieldComparison<PointT> ("x", pcl::ComparisonOps::GE, origin.x))); range_cond->addComparison (FieldComparisonConstPtr (new pcl::FieldComparison<PointT> ("x", pcl::ComparisonOps::LT, origin.x + cubeSide))); range_cond->addComparison (FieldComparisonConstPtr (new pcl::FieldComparison<PointT> ("y", pcl::ComparisonOps::GE, origin.y))); range_cond->addComparison (FieldComparisonConstPtr (new pcl::FieldComparison<PointT> ("y", pcl::ComparisonOps::LT, origin.y + cubeSide))); range_cond->addComparison (FieldComparisonConstPtr (new pcl::FieldComparison<PointT> ("z", pcl::ComparisonOps::GE, origin.z))); range_cond->addComparison (FieldComparisonConstPtr (new pcl::FieldComparison<PointT> ("z", pcl::ComparisonOps::LT, origin.z + cubeSide))); // build the filter pcl::ConditionalRemoval<PointT> condrem; condrem.setCondition (range_cond); condrem.setInputCloud (world_); condrem.setKeepOrganized(false); // apply filter condrem.filter (*box); // also push transform along with points. if(box->points.size() > 0) { Eigen::Vector3f transform; transform[0] = origin.x, transform[1] = origin.y, transform[2] = origin.z; transforms.push_back(transform); cubes.push_back(box); } else { PCL_INFO ("Extracted cube was empty, skiping this one.\n"); } origin.z += cubeSide * step_increment; } origin.y += cubeSide * step_increment; } origin.x += cubeSide * step_increment; } /* for(int c = 0 ; c < cubes.size() ; ++c) { std::stringstream name; name << "cloud" << c+1 << ".pcd"; pcl::io::savePCDFileASCII(name.str(), *(cubes[c])); }*/ std::cout << "returning " << cubes.size() << " cubes" << std::endl; }
template <typename PointSource, typename PointTarget> void pcl::IterativeClosestPoint<PointSource, PointTarget>::computeTransformation (PointCloudSource &output, const Eigen::Matrix4f &guess) { // Allocate enough space to hold the results std::vector<int> nn_indices (1); std::vector<float> nn_dists (1); // Point cloud containing the correspondences of each point in <input, indices> PointCloudTarget input_corresp; input_corresp.points.resize (indices_->size ()); nr_iterations_ = 0; converged_ = false; double dist_threshold = corr_dist_threshold_ * corr_dist_threshold_; // If the guessed transformation is non identity if (guess != Eigen::Matrix4f::Identity ()) { // Initialise final transformation to the guessed one final_transformation_ = guess; // Apply guessed transformation prior to search for neighbours transformPointCloud (output, output, guess); } // Resize the vector of distances between correspondences std::vector<float> previous_correspondence_distances (indices_->size ()); correspondence_distances_.resize (indices_->size ()); while (!converged_) // repeat until convergence { // Save the previously estimated transformation previous_transformation_ = transformation_; // And the previous set of distances previous_correspondence_distances = correspondence_distances_; int cnt = 0; std::vector<int> source_indices (indices_->size ()); std::vector<int> target_indices (indices_->size ()); // Iterating over the entire index vector and find all correspondences for (size_t idx = 0; idx < indices_->size (); ++idx) { if (!this->searchForNeighbors (output, (*indices_)[idx], nn_indices, nn_dists)) { PCL_ERROR ("[pcl::%s::computeTransformation] Unable to find a nearest neighbor in the target dataset for point %d in the source!\n", getClassName ().c_str (), (*indices_)[idx]); return; } // Check if the distance to the nearest neighbor is smaller than the user imposed threshold if (nn_dists[0] < dist_threshold) { source_indices[cnt] = (*indices_)[idx]; target_indices[cnt] = nn_indices[0]; cnt++; } // Save the nn_dists[0] to a global vector of distances correspondence_distances_[(*indices_)[idx]] = std::min (nn_dists[0], (float)dist_threshold); } if (cnt < min_number_correspondences_) { PCL_ERROR ("[pcl::%s::computeTransformation] Not enough correspondences found. Relax your threshold parameters.\n", getClassName ().c_str ()); converged_ = false; return; } // Resize to the actual number of valid correspondences source_indices.resize (cnt); target_indices.resize (cnt); std::vector<int> source_indices_good; std::vector<int> target_indices_good; { // From the set of correspondences found, attempt to remove outliers // Create the registration model typedef typename SampleConsensusModelRegistration<PointSource>::Ptr SampleConsensusModelRegistrationPtr; SampleConsensusModelRegistrationPtr model; model.reset (new SampleConsensusModelRegistration<PointSource> (output.makeShared (), source_indices)); // Pass the target_indices model->setInputTarget (target_, target_indices); // Create a RANSAC model RandomSampleConsensus<PointSource> sac (model, inlier_threshold_); sac.setMaxIterations (ransac_iterations_); // Compute the set of inliers if (!sac.computeModel ()) { source_indices_good = source_indices; target_indices_good = target_indices; } else { std::vector<int> inliers; // Get the inliers sac.getInliers (inliers); source_indices_good.resize (inliers.size ()); target_indices_good.resize (inliers.size ()); boost::unordered_map<int, int> source_to_target; for (unsigned int i = 0; i < source_indices.size(); ++i) source_to_target[source_indices[i]] = target_indices[i]; // Copy just the inliers std::copy(inliers.begin(), inliers.end(), source_indices_good.begin()); for (size_t i = 0; i < inliers.size (); ++i) target_indices_good[i] = source_to_target[inliers[i]]; } } // Check whether we have enough correspondences cnt = (int)source_indices_good.size (); if (cnt < min_number_correspondences_) { PCL_ERROR ("[pcl::%s::computeTransformation] Not enough correspondences found. Relax your threshold parameters.\n", getClassName ().c_str ()); converged_ = false; return; } PCL_DEBUG ("[pcl::%s::computeTransformation] Number of correspondences %d [%f%%] out of %lu points [100.0%%], RANSAC rejected: %lu [%f%%].\n", getClassName ().c_str (), cnt, (cnt * 100.0) / indices_->size (), (unsigned long)indices_->size (), (unsigned long)source_indices.size () - cnt, (source_indices.size () - cnt) * 100.0 / source_indices.size ()); // Estimate the transform //rigid_transformation_estimation_(output, source_indices_good, *target_, target_indices_good, transformation_); transformation_estimation_->estimateRigidTransformation (output, source_indices_good, *target_, target_indices_good, transformation_); // Tranform the data transformPointCloud (output, output, transformation_); // Obtain the final transformation final_transformation_ = transformation_ * final_transformation_; nr_iterations_++; // Update the vizualization of icp convergence if (update_visualizer_ != 0) update_visualizer_(output, source_indices_good, *target_, target_indices_good ); // Various/Different convergence termination criteria // 1. Number of iterations has reached the maximum user imposed number of iterations (via // setMaximumIterations) // 2. The epsilon (difference) between the previous transformation and the current estimated transformation // is smaller than an user imposed value (via setTransformationEpsilon) // 3. The sum of Euclidean squared errors is smaller than a user defined threshold (via // setEuclideanFitnessEpsilon) if (nr_iterations_ >= max_iterations_ || fabs ((transformation_ - previous_transformation_).sum ()) < transformation_epsilon_ || fabs (this->getFitnessScore (correspondence_distances_, previous_correspondence_distances)) <= euclidean_fitness_epsilon_ ) { converged_ = true; PCL_DEBUG ("[pcl::%s::computeTransformation] Convergence reached. Number of iterations: %d out of %d. Transformation difference: %f\n", getClassName ().c_str (), nr_iterations_, max_iterations_, fabs ((transformation_ - previous_transformation_).sum ())); PCL_DEBUG ("nr_iterations_ (%d) >= max_iterations_ (%d)\n", nr_iterations_, max_iterations_); PCL_DEBUG ("fabs ((transformation_ - previous_transformation_).sum ()) (%f) < transformation_epsilon_ (%f)\n", fabs ((transformation_ - previous_transformation_).sum ()), transformation_epsilon_); PCL_DEBUG ("fabs (getFitnessScore (correspondence_distances_, previous_correspondence_distances)) (%f) <= euclidean_fitness_epsilon_ (%f)\n", fabs (this->getFitnessScore (correspondence_distances_, previous_correspondence_distances)), euclidean_fitness_epsilon_); } } }
Eigen::Matrix4f Calibration::stitch(pcl::PointCloud<PointT>& points, double epsilon, double maxCorrespondanceDistance) { if (this->stitching.size() == 0) { pcl::copyPointCloud(points, this->stitching); return Eigen::Matrix4f::Identity(); // Hardcore hack !! } // Compute surface normals and curvature pcl::PointCloud<PointT> tempTarget; pcl::copyPointCloud(this->stitching, tempTarget); pcl::PointCloud<pcl::PointNormal>::Ptr pointsWithNormalSource (new pcl::PointCloud<pcl::PointNormal>); pcl::PointCloud<pcl::PointNormal>::Ptr pointsWithNormalTarget (new pcl::PointCloud<pcl::PointNormal>); pcl::NormalEstimation<PointT, pcl::PointNormal> normalEstimate; pcl::search::KdTree<PointT>::Ptr tree (new pcl::search::KdTree<PointT> ()); normalEstimate.setSearchMethod(tree); normalEstimate.setKSearch(30); normalEstimate.setInputCloud(points.makeShared()); normalEstimate.compute(*pointsWithNormalSource); pcl::copyPointCloud(points, *pointsWithNormalSource); normalEstimate.setInputCloud (tempTarget.makeShared()); normalEstimate.compute(*pointsWithNormalTarget); pcl::copyPointCloud (tempTarget, *pointsWithNormalTarget); // Instantiate custom point representation MyPointNormal pointNormal; // ... and weight the 'curvature' dimension so that it is balanced against x, y, and z float alpha[4] = {1.0, 1.0, 1.0, 1.0}; pointNormal.setRescaleValues(alpha); // Align pcl::IterativeClosestPointNonLinear<pcl::PointNormal, pcl::PointNormal> registration; registration.setTransformationEpsilon(epsilon); // Set the maximum distance between two correspondences registration.setMaxCorrespondenceDistance(maxCorrespondanceDistance); // Set the point representation registration.setPointRepresentation (boost::make_shared<const MyPointNormal> (pointNormal)); registration.setInputSource(pointsWithNormalSource); registration.setInputTarget(pointsWithNormalTarget); registration.setMaximumIterations(30); PCL_ERROR("Source size: %d -- Target size: %d\n", (int)pointsWithNormalSource.get()->size(), (int)pointsWithNormalTarget.get()->size()); Eigen::Matrix4f tf = Eigen::Matrix4f::Identity(); pcl::PointCloud<pcl::PointNormal>::Ptr regResult = pointsWithNormalSource; PCL_ERROR("Stitching ... "); registration.align(*regResult); PCL_ERROR("Done!\n"); tf = registration.getFinalTransformation().inverse(); // PCL_ERROR("\nTransform:\n"); // PCL_ERROR("| %f %f %f %f |\n", tf(0,0), tf(0,1), tf (0,2), tf(0,3)); // PCL_ERROR("| %f %f %f %f |\n", tf(1,0), tf(1,1), tf (1,2), tf(1,3)); // PCL_ERROR("| %f %f %f %f |\n", tf(2,0), tf(2,1), tf (2,2), tf(2,3)); // PCL_ERROR("| %f %f %f %f |\n\n", tf(3,0), tf(3,1), tf (3,2), tf(3,3)); pcl::transformPointCloud(*pointsWithNormalSource, *regResult, tf); *regResult += *pointsWithNormalTarget; pcl::copyPointCloud(*regResult, this->stitching); return tf; }
int main (int argc, char** argv) { // ///////////////////////////////////////////////////////////////////////////////////////////////////// if (argc != 3) { std::cerr << "please specify the paths to the two point clouds to be registered" << std::endl; exit (0); } // ///////////////////////////////////////////////////////////////////////////////////////////////////// // ///////////////////////////////////////////////////////////////////////////////////////////////////// pcl::PointCloud<pcl::PointXYZ> inputCloud; pcl::PointCloud<pcl::PointXYZ> targetCloud; if (pcl::io::loadPCDFile<pcl::PointXYZ> (argv[1], inputCloud) == -1) //* load the file { PCL_ERROR ("Couldn't read the first .pcd file \n"); return (-1); } if (pcl::io::loadPCDFile<pcl::PointXYZ> (argv[2], targetCloud) == -1) //* load the file { PCL_ERROR ("Couldn't read the second .pcd file \n"); return (-1); } // ///////////////////////////////////////////////////////////////////////////////////////////////////// // ///////////////////////////////////////////////////////////////////////////////////////////////////// pcl::PointCloud<pcl::PointXYZ> inputCloudFiltered; pcl::PointCloud<pcl::PointXYZ> targetCloudFiltered; pcl::VoxelGrid<pcl::PointXYZ> sor; // sor.setLeafSize (0.01, 0.01, 0.01); sor.setLeafSize (0.02f, 0.02f, 0.02f); // sor.setLeafSize (0.05, 0.05, 0.05); // sor.setLeafSize (0.1, 0.1, 0.1); // sor.setLeafSize (0.4, 0.4, 0.4); // sor.setLeafSize (0.5, 0.5, 0.5); sor.setInputCloud (inputCloud.makeShared()); std::cout<<"\n inputCloud.size()="<<inputCloud.size()<<std::endl; sor.filter (inputCloudFiltered); std::cout<<"\n inputCloudFiltered.size()="<<inputCloudFiltered.size()<<std::endl; sor.setInputCloud (targetCloud.makeShared()); std::cout<<"\n targetCloud.size()="<<targetCloud.size()<<std::endl; sor.filter (targetCloudFiltered); std::cout<<"\n targetCloudFiltered.size()="<<targetCloudFiltered.size()<<std::endl; // ///////////////////////////////////////////////////////////////////////////////////////////////////// // ///////////////////////////////////////////////////////////////////////////////////////////////////// // pcl::PointCloud<pcl::PointXYZ>::ConstPtr source = inputCloud.makeShared(); // pcl::PointCloud<pcl::PointXYZ>::ConstPtr target = targetCloud.makeShared(); pcl::PointCloud<pcl::PointXYZ>::ConstPtr source = inputCloudFiltered.makeShared(); pcl::PointCloud<pcl::PointXYZ>::ConstPtr target = targetCloudFiltered.makeShared(); pcl::PointCloud<pcl::PointXYZ> source_aligned; // ///////////////////////////////////////////////////////////////////////////////////////////////////// // ///////////////////////////////////////////////////////////////////////////////////////////////////// pcl::RegistrationVisualizer<pcl::PointXYZ, pcl::PointXYZ> registrationVisualizer; registrationVisualizer.startDisplay(); registrationVisualizer.setMaximumDisplayedCorrespondences (100); // ///////////////////////////////////////////////////////////////////////////////////////////////////// // /////////////////////////////////////////////////////////////////////////////////////////////////// pcl::IterativeClosestPoint<pcl::PointXYZ, pcl::PointXYZ> icp; icp.setMaximumIterations(10000); // icp.setMaxCorrespondenceDistance (0.6); icp.setMaxCorrespondenceDistance (0.8); // icp.setMaxCorrespondenceDistance (1.5); // icp.setRANSACOutlierRejectionThreshold (0.1); icp.setRANSACOutlierRejectionThreshold (0.6); // icp.setRANSACOutlierRejectionThreshold (1.5); // icp.setRANSACOutlierRejectionThreshold (5.0); icp.setInputCloud (source); icp.setInputTarget (target); // Register the registration algorithm to the RegistrationVisualizer registrationVisualizer.setRegistration (icp); // Start registration process icp.align (source_aligned); std::cout << "has converged:" << icp.hasConverged () << " score: " << icp.getFitnessScore () << std::endl; std::cout << icp.getFinalTransformation () << std::endl; // /////////////////////////////////////////////////////////////////////////////////////////////////// // /////////////////////////////////////////////////////////////////////////////////////////////////// // // pcl::IterativeClosestPointNonLinear<pcl::PointXYZ, pcl::PointXYZ> icpnl; // // icpnl.setMaximumIterations(10000); // // icpnl.setMaxCorrespondenceDistance (0.6); //// icpnl.setMaxCorrespondenceDistance (0.8); //// icpnl.setMaxCorrespondenceDistance (1.5); // //// icpnl.setRANSACOutlierRejectionThreshold (0.1); // icpnl.setRANSACOutlierRejectionThreshold (0.8); //// icpnl.setRANSACOutlierRejectionThreshold (1.5); //// icpnl.setRANSACOutlierRejectionThreshold (5.0); // // icpnl.setInputCloud (source); // icpnl.setInputTarget (target); // // // Register the registration algorithm to the RegistrationVisualizer // registrationVisualizer.setRegistration (icpnl); // // // Start registration process // icpnl.align (source_aligned); // // std::cout << "has converged:" << icpnl.hasConverged () << " score: " << icpnl.getFitnessScore () << std::endl; // std::cout << icpnl.getFinalTransformation () << std::endl; // // /////////////////////////////////////////////////////////////////////////////////////////////////// }
////////////////////////////////////////////////////////////////////////////// //const pcl::PointCloud<pcl::PointXYZRGBA>::Ptr pcl::StereoMatching::getPointCloud(float uC, float vC, float focal, float baseline) bool pcl::StereoMatching::getPointCloud ( float u_c, float v_c, float focal, float baseline, pcl::PointCloud<pcl::PointXYZ>::Ptr cloud) { //disp map has not been computed yet.. if ( disp_map_ == NULL) { PCL_ERROR( "[pcl::StereoMatching::getPointCloud] Error: a disparity map has not been computed yet. The resulting cloud can not be computed..\n" ); return false; } //cloud needs to be re-allocated if (cloud->width != width_ || cloud->height != height_) { cloud->resize(width_*height_); cloud->width = width_; cloud->height = height_; cloud->is_dense = false; } if ( cloud->is_dense) cloud->is_dense = false; //Loop pcl::PointXYZ temp_point; pcl::PointXYZ nan_point; nan_point.x = std::numeric_limits<float>::quiet_NaN(); nan_point.y = std::numeric_limits<float>::quiet_NaN(); nan_point.z = std::numeric_limits<float>::quiet_NaN(); //nan_point.intensity = std::numeric_limits<float>::quiet_NaN(); //all disparities are multiplied by a constant equal to 16; //this must be taken into account when computing z values float depth_scale = baseline * focal * 16.0f; for ( int j=0; j<height_; j++) { for ( int i=0; i<width_; i++) { if ( disp_map_[ j*width_ + i] > 0 ) { temp_point.z = depth_scale / disp_map_[j * width_ + i]; temp_point.x = ((static_cast<float> (i) - u_c) * temp_point.z) / focal; temp_point.y = ((static_cast<float> (j) - v_c) * temp_point.z) / focal; //temp_point.intensity = 255; (*cloud)[j * width_ + i] = temp_point; } //adding NaN value else { (*cloud)[j * width_ + i] = nan_point; } } } return (true); }
void pcl::GrayStereoMatching::compute (unsigned char* ref_img, unsigned char* trg_img, int width, int height) { //Check that a suitable value of max_disp has been selected if ( max_disp_ <= 0) { PCL_ERROR( "[pcl::StereoMatching::compute] Error. A positive max_disparity value has not be correctly inserted. Aborting..\n" ); return; } if ( (disp_map_ != NULL) && (width_ != width || height_ != height) ) { delete [] disp_map_; disp_map_ = NULL; if ( disp_map_trg_ != NULL) { delete [] disp_map_trg_; disp_map_trg_ = NULL; } if ( pp_ref_img_ != NULL) { delete [] pp_ref_img_; delete [] pp_trg_img_; pp_ref_img_ = NULL; pp_trg_img_ = NULL; } } if ( disp_map_ == NULL) { disp_map_ = new short int[width * height]; width_ = width; height_ = height; } if ( is_lr_check_ && disp_map_trg_ == NULL) { disp_map_trg_ = new short int[width * height]; } if ( !is_lr_check_ && disp_map_trg_ != NULL) { delete [] disp_map_trg_; disp_map_trg_ = NULL; } if ( is_pre_proc_ && pp_ref_img_ == NULL) { pp_ref_img_ = new unsigned char[width_*height_]; pp_trg_img_ = new unsigned char[width_*height_]; } if ( !is_pre_proc_ && pp_ref_img_ != NULL) { delete [] pp_ref_img_; delete [] pp_trg_img_; pp_ref_img_ = NULL; pp_trg_img_ = NULL; } memset(disp_map_, 0, sizeof(short int)*height_*width_); if ( is_pre_proc_) { preProcessing(ref_img, pp_ref_img_); preProcessing(trg_img, pp_trg_img_); } if (is_lr_check_) { if ( is_pre_proc_) { imgFlip(pp_ref_img_); imgFlip(pp_trg_img_); compute_impl(pp_trg_img_, pp_ref_img_); imgFlip(pp_ref_img_); imgFlip(pp_trg_img_); } else { imgFlip(ref_img); imgFlip(trg_img); compute_impl(trg_img, ref_img); imgFlip(ref_img); imgFlip(trg_img); } for (int j = 0; j < height_; j++) for (int i = 0; i < width_; i++) disp_map_trg_[j * width_ + i] = disp_map_[j * width_ + width_ - 1 - i]; } if ( is_pre_proc_) compute_impl(pp_ref_img_, pp_trg_img_); else compute_impl(ref_img, trg_img); if ( is_lr_check_) { leftRightCheck(); } //at the end, x_offset (*16) needs to be added to all computed disparities, //so that each fixed point value of the disparity map represents the true disparity value multiplied by 16 for (int j = 0; j < height_; j++) for (int i = 0; i < width_; i++) if ( disp_map_[j * width_ + i] > 0) disp_map_[j * width_ + i] += static_cast<short int> (x_off_ * 16); }
bool pcl::StereoMatching::getPointCloud ( float u_c, float v_c, float focal, float baseline, pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud, pcl::PointCloud<pcl::RGB>::Ptr texture) { //disp map has not been computed yet.. if (disp_map_ == NULL) { PCL_ERROR ("[pcl::StereoMatching::getPointCloud] Error: a disparity map has not been computed yet. The resulting cloud can not be computed..\n"); return (false); } if (texture->width != width_ || texture->height != height_) { PCL_ERROR("[pcl::StereoMatching::getPointCloud] Error: the size of the texture cloud does not match that of the computed range map. The resulting cloud can not be computed..\n"); return (false); } //cloud needs to be re-allocated if (cloud->width != width_ || cloud->height != height_) { //cloud.reset(new pcl::PointCloud<pcl::PointXYZRGBA>(width_, height_) ); cloud->resize (width_ * height_); cloud->width = width_; cloud->height = height_; cloud->is_dense = false; } //Loop pcl::PointXYZRGB temp_point; /*pcl::PointXYZRGB nan_point; nan_point.x = std::numeric_limits<float>::quiet_NaN(); nan_point.y = std::numeric_limits<float>::quiet_NaN(); nan_point.z = std::numeric_limits<float>::quiet_NaN(); nan_point.r = std::numeric_limits<unsigned char>::quiet_NaN(); nan_point.g = std::numeric_limits<unsigned char>::quiet_NaN(); nan_point.b = std::numeric_limits<unsigned char>::quiet_NaN();*/ //all disparities are multiplied by a constant equal to 16; //this must be taken into account when computing z values float depth_scale = baseline * focal * 16.0f; for (int j = 0; j < height_; j++) { for (int i = 0; i < width_; i++) { if (disp_map_[ j*width_ + i] > 0) { temp_point.z = (depth_scale) / (disp_map_[ j*width_ + i]); temp_point.x = ((static_cast<float> (i) - u_c) * temp_point.z) / focal; temp_point.y = ((static_cast<float> (j) - v_c) * temp_point.z) / focal; //temp_point.intensity = ( texture->at(j*width_+i).r +texture->at(j*width_+i).g + texture->at(j*width_+i).b) / 3.0f; temp_point.r = texture->at (j * width_ + i).r; temp_point.g = texture->at (j * width_ + i).g; temp_point.b = texture->at (j * width_ + i).b; (*cloud)[j*width_ + i] = temp_point; } //adding NaN value else { temp_point.x = std::numeric_limits<float>::quiet_NaN(); temp_point.y = std::numeric_limits<float>::quiet_NaN(); temp_point.z = std::numeric_limits<float>::quiet_NaN(); temp_point.r = texture->at (j * width_ + i).r; temp_point.g = texture->at (j * width_ + i).g; temp_point.b = texture->at (j * width_ + i).b; (*cloud)[j*width_ + i] = temp_point; } } } return (true); }
bool pcl::concatenateFields (const sensor_msgs::PointCloud2 &cloud1, const sensor_msgs::PointCloud2 &cloud2, sensor_msgs::PointCloud2 &cloud_out) { // If the cloud's sizes differ (points wise), then exit with error if (cloud1.width != cloud2.width || cloud1.height != cloud2.height) { PCL_ERROR ("[pcl::concatenateFields] Dimensions of input clouds do not match: cloud1 (w, %d, h, %d), cloud2 (w, %d, h, %d)\n", cloud1.width, cloud1.height, cloud2.width, cloud2.height ); return (false); } if (cloud1.is_bigendian != cloud2.is_bigendian) { PCL_ERROR ("[pcl::concatenateFields] Endianness of clouds does not match\n"); return (false); } // Else, copy the second cloud (width, height, header stay the same) // we do this since fields from the second cloud are supposed to overwrite // those of the first cloud_out.header = cloud2.header; cloud_out.fields = cloud2.fields; cloud_out.width = cloud2.width; cloud_out.height = cloud2.height; cloud_out.is_bigendian = cloud2.is_bigendian; //We need to find how many fields overlap between the two clouds size_t total_fields = cloud2.fields.size (); //for the non-matching fields in cloud1, we need to store the offset //from the beginning of the point std::vector<const sensor_msgs::PointField*> cloud1_unique_fields; std::vector<int> field_sizes; //We need to make sure that the fields for cloud 1 are sorted //by offset so that we can compute sizes correctly. There is no //guarantee that the fields are in the correct order when they come in std::vector<const sensor_msgs::PointField*> cloud1_fields_sorted; for (size_t i = 0; i < cloud1.fields.size (); ++i) cloud1_fields_sorted.push_back (&(cloud1.fields[i])); std::sort (cloud1_fields_sorted.begin (), cloud1_fields_sorted.end (), fieldComp); for (size_t i = 0; i < cloud1_fields_sorted.size (); ++i) { bool match = false; for (size_t j = 0; j < cloud2.fields.size (); ++j) { if (cloud1_fields_sorted[i]->name == cloud2.fields[j].name) match = true; } //if the field is new, we'll increment out total fields if (!match && cloud1_fields_sorted[i]->name != "_") { cloud1_unique_fields.push_back (cloud1_fields_sorted[i]); int size = 0; size_t next_valid_field = i + 1; while (next_valid_field < cloud1_fields_sorted.size()) { if (cloud1_fields_sorted[next_valid_field]->name != "_") break; next_valid_field++; } if (next_valid_field < cloud1_fields_sorted.size ()) //compute the true size of the field, including padding size = cloud1_fields_sorted[next_valid_field]->offset - cloud1_fields_sorted[i]->offset; else //for the last point, we'll just use the point step to compute the size size = cloud1.point_step - cloud1_fields_sorted[i]->offset; field_sizes.push_back (size); total_fields++; } } //we need to compute the size of the additional data added from cloud 1 uint32_t cloud1_unique_point_step = 0; for (size_t i = 0; i < cloud1_unique_fields.size (); ++i) cloud1_unique_point_step += field_sizes[i]; //the total size of extra data should be the size of data per point //multiplied by the total number of points in the cloud uint32_t cloud1_unique_data_size = cloud1_unique_point_step * cloud1.width * cloud1.height; // Point step must increase with the length of each matching field cloud_out.point_step = cloud2.point_step + cloud1_unique_point_step; // Recalculate row_step cloud_out.row_step = cloud_out.point_step * cloud_out.width; // Resize data to hold all clouds cloud_out.data.resize (cloud2.data.size () + cloud1_unique_data_size); // Concatenate fields cloud_out.fields.resize (cloud2.fields.size () + cloud1_unique_fields.size ()); int offset = cloud2.point_step; for (size_t d = 0; d < cloud1_unique_fields.size (); ++d) { const sensor_msgs::PointField& f = *cloud1_unique_fields[d]; cloud_out.fields[cloud2.fields.size () + d].name = f.name; cloud_out.fields[cloud2.fields.size () + d].datatype = f.datatype; cloud_out.fields[cloud2.fields.size () + d].count = f.count; // Adjust the offset cloud_out.fields[cloud2.fields.size () + d].offset = offset; offset += field_sizes[d]; } // Iterate over each point and perform the appropriate memcpys int point_offset = 0; for (size_t cp = 0; cp < cloud_out.width * cloud_out.height; ++cp) { memcpy (&cloud_out.data[point_offset], &cloud2.data[cp * cloud2.point_step], cloud2.point_step); int field_offset = cloud2.point_step; // Copy each individual point, we have to do this on a per-field basis // since some fields are not unique for (size_t i = 0; i < cloud1_unique_fields.size (); ++i) { const sensor_msgs::PointField& f = *cloud1_unique_fields[i]; int local_data_size = f.count * pcl::getFieldSize (f.datatype); int padding_size = field_sizes[i] - local_data_size; memcpy (&cloud_out.data[point_offset + field_offset], &cloud1.data[cp * cloud1.point_step + f.offset], local_data_size); field_offset += local_data_size; //make sure that we add padding when its needed if (padding_size > 0) memset (&cloud_out.data[point_offset + field_offset], 0, padding_size); field_offset += padding_size; } point_offset += field_offset; } if (!cloud1.is_dense || !cloud2.is_dense) cloud_out.is_dense = false; else cloud_out.is_dense = true; return (true); }
template <typename PointXYZT, typename PointRGBT> bool pcl::LineRGBD<PointXYZT, PointRGBT>::loadTemplates (const std::string &file_name, const size_t object_id) { // Open the file int ltm_fd = pcl_open (file_name.c_str (), O_RDONLY); if (ltm_fd == -1) return (false); int ltm_offset = 0; pcl::io::TARHeader ltm_header; PCDReader pcd_reader; std::string pcd_ext (".pcd"); std::string sqmmt_ext (".sqmmt"); // While there still is an LTM header to be read while (readLTMHeader (ltm_fd, ltm_header)) { ltm_offset += 512; // Search for extension std::string chunk_name (ltm_header.file_name); std::transform (chunk_name.begin (), chunk_name.end (), chunk_name.begin (), ::tolower); std::string::size_type it; if ((it = chunk_name.find (pcd_ext)) != std::string::npos && (pcd_ext.size () - (chunk_name.size () - it)) == 0) { PCL_DEBUG ("[pcl::LineRGBD::loadTemplates] Reading and parsing %s as a PCD file.\n", chunk_name.c_str ()); // Read the next PCD file template_point_clouds_.resize (template_point_clouds_.size () + 1); pcd_reader.read (file_name, template_point_clouds_[template_point_clouds_.size () - 1], ltm_offset); // Increment the offset for the next file ltm_offset += (ltm_header.getFileSize ()) + (512 - ltm_header.getFileSize () % 512); } else if ((it = chunk_name.find (sqmmt_ext)) != std::string::npos && (sqmmt_ext.size () - (chunk_name.size () - it)) == 0) { PCL_DEBUG ("[pcl::LineRGBD::loadTemplates] Reading and parsing %s as a SQMMT file.\n", chunk_name.c_str ()); unsigned int fsize = ltm_header.getFileSize (); char *buffer = new char[fsize]; int result = static_cast<int> (::read (ltm_fd, reinterpret_cast<char*> (&buffer[0]), fsize)); if (result == -1) { delete [] buffer; PCL_ERROR ("[pcl::LineRGBD::loadTemplates] Error reading SQMMT template from file!\n"); break; } // Read a SQMMT file std::stringstream stream (std::stringstream::in | std::stringstream::out | std::stringstream::binary); stream.write (buffer, fsize); SparseQuantizedMultiModTemplate sqmmt; sqmmt.deserialize (stream); linemod_.addTemplate (sqmmt); object_ids_.push_back (object_id); // Increment the offset for the next file ltm_offset += (ltm_header.getFileSize ()) + (512 - ltm_header.getFileSize () % 512); delete [] buffer; } if (static_cast<int> (pcl_lseek (ltm_fd, ltm_offset, SEEK_SET)) < 0) break; } // Close the file pcl_close (ltm_fd); // Compute 3D bounding boxes from the template point clouds bounding_boxes_.resize (template_point_clouds_.size ()); for (size_t i = 0; i < template_point_clouds_.size (); ++i) { PointCloud<PointXYZRGBA> & template_point_cloud = template_point_clouds_[i]; BoundingBoxXYZ & bb = bounding_boxes_[i]; bb.x = bb.y = bb.z = std::numeric_limits<float>::max (); bb.width = bb.height = bb.depth = 0.0f; float center_x = 0.0f; float center_y = 0.0f; float center_z = 0.0f; float min_x = std::numeric_limits<float>::max (); float min_y = std::numeric_limits<float>::max (); float min_z = std::numeric_limits<float>::max (); float max_x = -std::numeric_limits<float>::max (); float max_y = -std::numeric_limits<float>::max (); float max_z = -std::numeric_limits<float>::max (); size_t counter = 0; for (size_t j = 0; j < template_point_cloud.size (); ++j) { const PointXYZRGBA & p = template_point_cloud.points[j]; if (!pcl_isfinite (p.x) || !pcl_isfinite (p.y) || !pcl_isfinite (p.z)) continue; min_x = std::min (min_x, p.x); min_y = std::min (min_y, p.y); min_z = std::min (min_z, p.z); max_x = std::max (max_x, p.x); max_y = std::max (max_y, p.y); max_z = std::max (max_z, p.z); center_x += p.x; center_y += p.y; center_z += p.z; ++counter; } center_x /= static_cast<float> (counter); center_y /= static_cast<float> (counter); center_z /= static_cast<float> (counter); bb.width = max_x - min_x; bb.height = max_y - min_y; bb.depth = max_z - min_z; bb.x = (min_x + bb.width / 2.0f) - center_x - bb.width / 2.0f; bb.y = (min_y + bb.height / 2.0f) - center_y - bb.height / 2.0f; bb.z = (min_z + bb.depth / 2.0f) - center_z - bb.depth / 2.0f; for (size_t j = 0; j < template_point_cloud.size (); ++j) { PointXYZRGBA p = template_point_cloud.points[j]; if (!pcl_isfinite (p.x) || !pcl_isfinite (p.y) || !pcl_isfinite (p.z)) continue; p.x -= center_x; p.y -= center_y; p.z -= center_z; template_point_cloud.points[j] = p; } } return (true); }
void pcl::RadiusOutlierRemoval<sensor_msgs::PointCloud2>::applyFilter (PointCloud2 &output) { output.is_dense = true; // If fields x/y/z are not present, we cannot filter if (x_idx_ == -1 || y_idx_ == -1 || z_idx_ == -1) { PCL_ERROR ("[pcl::%s::applyFilter] Input dataset doesn't have x-y-z coordinates!\n", getClassName ().c_str ()); output.width = output.height = 0; output.data.clear (); return; } if (search_radius_ == 0.0) { PCL_ERROR ("[pcl::%s::applyFilter] No radius defined!\n", getClassName ().c_str ()); output.width = output.height = 0; output.data.clear (); return; } // Send the input dataset to the spatial locator pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>); pcl::fromROSMsg (*input_, *cloud); // Initialize the spatial locator if (!tree_) { if (cloud->isOrganized ()) tree_.reset (new pcl::search::OrganizedNeighbor<pcl::PointXYZ> ()); else tree_.reset (new pcl::search::KdTree<pcl::PointXYZ> (false)); } tree_->setInputCloud (cloud); // Allocate enough space to hold the results std::vector<int> nn_indices (indices_->size ()); std::vector<float> nn_dists (indices_->size ()); // Copy the common fields output.is_bigendian = input_->is_bigendian; output.point_step = input_->point_step; output.height = 1; output.data.resize (input_->width * input_->point_step); // reserve enough space removed_indices_->resize (input_->data.size ()); int nr_p = 0; int nr_removed_p = 0; // Go over all the points and check which doesn't have enough neighbors for (int cp = 0; cp < static_cast<int> (indices_->size ()); ++cp) { int k = tree_->radiusSearch ((*indices_)[cp], search_radius_, nn_indices, nn_dists); // Check if the number of neighbors is larger than the user imposed limit if (k < min_pts_radius_) { if (extract_removed_indices_) { (*removed_indices_)[nr_removed_p] = cp; nr_removed_p++; } continue; } memcpy (&output.data[nr_p * output.point_step], &input_->data[(*indices_)[cp] * output.point_step], output.point_step); nr_p++; } output.width = nr_p; output.height = 1; output.data.resize (output.width * output.point_step); output.row_step = output.point_step * output.width; removed_indices_->resize (nr_removed_p); }
template <typename PointT> bool pcl::registration::ELCH<PointT>::initCompute () { /*if (!PCLBase<PointT>::initCompute ()) { PCL_ERROR ("[pcl::registration:ELCH::initCompute] Init failed.\n"); return (false); }*/ //TODO if (!loop_start_) { PCL_ERROR ("[pcl::registration::ELCH::initCompute] no start of loop defined is empty!\n"); deinitCompute (); return (false); } if (!loop_end_) { PCL_ERROR ("[pcl::registration::ELCH::initCompute] no end of loop defined is empty!\n"); deinitCompute (); return (false); } //compute transformation if it's not given if (compute_loop_) { PointCloudPtr meta_start (new PointCloud); PointCloudPtr meta_end (new PointCloud); *meta_start = *(*loop_graph_)[loop_start_].cloud; *meta_end = *(*loop_graph_)[loop_end_].cloud; typename boost::graph_traits<LoopGraph>::adjacency_iterator si, si_end; for (boost::tuples::tie (si, si_end) = adjacent_vertices (loop_start_, *loop_graph_); si != si_end; si++) *meta_start += *(*loop_graph_)[*si].cloud; for (boost::tuples::tie (si, si_end) = adjacent_vertices (loop_end_, *loop_graph_); si != si_end; si++) *meta_end += *(*loop_graph_)[*si].cloud; //TODO use real pose instead of centroid //Eigen::Vector4f pose_start; //pcl::compute3DCentroid (*(*loop_graph_)[loop_start_].cloud, pose_start); //Eigen::Vector4f pose_end; //pcl::compute3DCentroid (*(*loop_graph_)[loop_end_].cloud, pose_end); PointCloudPtr tmp (new PointCloud); //Eigen::Vector4f diff = pose_start - pose_end; //Eigen::Translation3f translation (diff.head (3)); //Eigen::Affine3f trans = translation * Eigen::Quaternionf::Identity (); //pcl::transformPointCloud (*(*loop_graph_)[loop_end_].cloud, *tmp, trans); reg_->setInputTarget (meta_start); reg_->setInputCloud (meta_end); reg_->align (*tmp); loop_transform_ = reg_->getFinalTransformation (); //TODO hack //loop_transform_ *= trans.matrix (); } return (true); }
template <typename PointT> bool pcl::MaximumLikelihoodSampleConsensus<PointT>::computeModel (int debug_verbosity_level) { // Warn and exit if no threshold was set if (threshold_ == std::numeric_limits<double>::max()) { PCL_ERROR ("[pcl::MaximumLikelihoodSampleConsensus::computeModel] No threshold set!\n"); return (false); } iterations_ = 0; double d_best_penalty = std::numeric_limits<double>::max(); double k = 1.0; std::vector<int> best_model; std::vector<int> selection; Eigen::VectorXf model_coefficients; std::vector<double> distances; // Compute sigma - remember to set threshold_ correctly ! sigma_ = computeMedianAbsoluteDeviation (sac_model_->getInputCloud (), sac_model_->getIndices (), threshold_); if (debug_verbosity_level > 1) PCL_DEBUG ("[pcl::MaximumLikelihoodSampleConsensus::computeModel] Estimated sigma value: %f.\n", sigma_); // Compute the bounding box diagonal: V = sqrt (sum (max(pointCloud) - min(pointCloud)^2)) Eigen::Vector4f min_pt, max_pt; getMinMax (sac_model_->getInputCloud (), sac_model_->getIndices (), min_pt, max_pt); max_pt -= min_pt; double v = sqrt (max_pt.dot (max_pt)); int n_inliers_count = 0; size_t indices_size; unsigned skipped_count = 0; // supress infinite loops by just allowing 10 x maximum allowed iterations for invalid model parameters! const unsigned max_skip = max_iterations_ * 10; // Iterate while (iterations_ < k && skipped_count < max_skip) { // Get X samples which satisfy the model criteria sac_model_->getSamples (iterations_, selection); if (selection.empty ()) break; // Search for inliers in the point cloud for the current plane model M if (!sac_model_->computeModelCoefficients (selection, model_coefficients)) { //iterations_++; ++ skipped_count; continue; } // Iterate through the 3d points and calculate the distances from them to the model sac_model_->getDistancesToModel (model_coefficients, distances); if (distances.empty ()) { //iterations_++; ++skipped_count; continue; } // Use Expectiation-Maximization to find out the right value for d_cur_penalty // ---[ Initial estimate for the gamma mixing parameter = 1/2 double gamma = 0.5; double p_outlier_prob = 0; indices_size = sac_model_->getIndices ()->size (); std::vector<double> p_inlier_prob (indices_size); for (int j = 0; j < iterations_EM_; ++j) { // Likelihood of a datum given that it is an inlier for (size_t i = 0; i < indices_size; ++i) p_inlier_prob[i] = gamma * exp (- (distances[i] * distances[i] ) / 2 * (sigma_ * sigma_) ) / (sqrt (2 * M_PI) * sigma_); // Likelihood of a datum given that it is an outlier p_outlier_prob = (1 - gamma) / v; gamma = 0; for (size_t i = 0; i < indices_size; ++i) gamma += p_inlier_prob [i] / (p_inlier_prob[i] + p_outlier_prob); gamma /= static_cast<double>(sac_model_->getIndices ()->size ()); } // Find the log likelihood of the model -L = -sum [log (pInlierProb + pOutlierProb)] double d_cur_penalty = 0; for (size_t i = 0; i < indices_size; ++i) d_cur_penalty += log (p_inlier_prob[i] + p_outlier_prob); d_cur_penalty = - d_cur_penalty; // Better match ? if (d_cur_penalty < d_best_penalty) { d_best_penalty = d_cur_penalty; // Save the current model/coefficients selection as being the best so far model_ = selection; model_coefficients_ = model_coefficients; n_inliers_count = 0; // Need to compute the number of inliers for this model to adapt k for (size_t i = 0; i < distances.size (); ++i) if (distances[i] <= 2 * sigma_) n_inliers_count++; // Compute the k parameter (k=log(z)/log(1-w^n)) double w = static_cast<double> (n_inliers_count) / static_cast<double> (sac_model_->getIndices ()->size ()); double p_no_outliers = 1 - pow (w, static_cast<double> (selection.size ())); p_no_outliers = (std::max) (std::numeric_limits<double>::epsilon (), p_no_outliers); // Avoid division by -Inf p_no_outliers = (std::min) (1 - std::numeric_limits<double>::epsilon (), p_no_outliers); // Avoid division by 0. k = log (1 - probability_) / log (p_no_outliers); } ++iterations_; if (debug_verbosity_level > 1) PCL_DEBUG ("[pcl::MaximumLikelihoodSampleConsensus::computeModel] Trial %d out of %d. Best penalty is %f.\n", iterations_, static_cast<int> (ceil (k)), d_best_penalty); if (iterations_ > max_iterations_) { if (debug_verbosity_level > 0) PCL_DEBUG ("[pcl::MaximumLikelihoodSampleConsensus::computeModel] MLESAC reached the maximum number of trials.\n"); break; } } if (model_.empty ()) { if (debug_verbosity_level > 0) PCL_DEBUG ("[pcl::MaximumLikelihoodSampleConsensus::computeModel] Unable to find a solution!\n"); return (false); } // Iterate through the 3d points and calculate the distances from them to the model again sac_model_->getDistancesToModel (model_coefficients_, distances); std::vector<int> &indices = *sac_model_->getIndices (); if (distances.size () != indices.size ()) { PCL_ERROR ("[pcl::MaximumLikelihoodSampleConsensus::computeModel] Estimated distances (%lu) differs than the normal of indices (%lu).\n", distances.size (), indices.size ()); return (false); } inliers_.resize (distances.size ()); // Get the inliers for the best model found n_inliers_count = 0; for (size_t i = 0; i < distances.size (); ++i) if (distances[i] <= 2 * sigma_) inliers_[n_inliers_count++] = indices[i]; // Resize the inliers vector inliers_.resize (n_inliers_count); if (debug_verbosity_level > 0) PCL_DEBUG ("[pcl::MaximumLikelihoodSampleConsensus::computeModel] Model: %lu size, %d inliers.\n", model_.size (), n_inliers_count); return (true); }
template <typename PointSource, typename PointTarget, typename FeatureT> void pcl::SampleConsensusInitialAlignment<PointSource, PointTarget, FeatureT>::computeTransformation (PointCloudSource &output, const Eigen::Matrix4f& guess) { if (!input_features_) { PCL_ERROR ("[pcl::%s::computeTransformation] ", getClassName ().c_str ()); PCL_ERROR ("No source features were given! Call setSourceFeatures before aligning.\n"); return; } if (!target_features_) { PCL_ERROR ("[pcl::%s::computeTransformation] ", getClassName ().c_str ()); PCL_ERROR ("No target features were given! Call setTargetFeatures before aligning.\n"); return; } if (!error_functor_) { error_functor_.reset (new TruncatedError (static_cast<float> (corr_dist_threshold_))); } std::vector<int> sample_indices (nr_samples_); std::vector<int> corresponding_indices (nr_samples_); PointCloudSource input_transformed; float error, lowest_error (0); final_transformation_ = guess; int i_iter = 0; if (!guess.isApprox(Eigen::Matrix4f::Identity (), 0.01f)) { //If guess is not the Identity matrix we check it. transformPointCloud (*input_, input_transformed, final_transformation_); lowest_error = computeErrorMetric (input_transformed, static_cast<float> (corr_dist_threshold_)); i_iter = 1; } for (; i_iter < max_iterations_; ++i_iter) { // Draw nr_samples_ random samples selectSamples (*input_, nr_samples_, min_sample_distance_, sample_indices); // Find corresponding features in the target cloud findSimilarFeatures (*input_features_, sample_indices, corresponding_indices); // Estimate the transform from the samples to their corresponding points transformation_estimation_->estimateRigidTransformation (*input_, sample_indices, *target_, corresponding_indices, transformation_); // Tranform the data and compute the error transformPointCloud (*input_, input_transformed, transformation_); error = computeErrorMetric (input_transformed, static_cast<float> (corr_dist_threshold_)); // If the new error is lower, update the final transformation if (i_iter == 0 || error < lowest_error) { lowest_error = error; final_transformation_ = transformation_; } } // Apply the final transformation transformPointCloud (*input_, output, final_transformation_); }
template <typename PointT, typename PointNT> bool pcl::SmoothedSurfacesKeypoint<PointT, PointNT>::initCompute () { PCL_INFO ("SmoothedSurfacesKeypoint initCompute () called\n"); if ( !Keypoint<PointT, PointT>::initCompute ()) { PCL_ERROR ("[pcl::SmoothedSurfacesKeypoints::initCompute] Keypoint::initCompute failed\n"); return false; } if (!normals_) { PCL_ERROR ("[pcl::SmoothedSurfacesKeypoints::initCompute] Input normals were not set\n"); return false; } if (clouds_.size () == 0) { PCL_ERROR ("[pcl::SmoothedSurfacesKeypoints::initCompute] No other clouds were set apart from the input\n"); return false; } if (input_->points.size () != normals_->points.size ()) { PCL_ERROR ("[pcl::SmoothedSurfacesKeypoints::initCompute] The input cloud and the input normals differ in size\n"); return false; } for (size_t cloud_i = 0; cloud_i < clouds_.size (); ++cloud_i) { if (clouds_[cloud_i]->points.size () != input_->points.size ()) { PCL_ERROR ("[pcl::SmoothedSurfacesKeypoints::initCompute] Cloud %d does not have the same number of points as the input cloud\n", cloud_i); return false; } if (cloud_normals_[cloud_i]->points.size () != input_->points.size ()) { PCL_ERROR ("[pcl::SmoothedSurfacesKeypoints::initCompute] Normals for cloud %d do not have the same number of points as the input cloud\n", cloud_i); return false; } } // Add the input cloud as the last index scales_.push_back (std::pair<float, size_t> (input_scale_, scales_.size ())); clouds_.push_back (input_); cloud_normals_.push_back (normals_); cloud_trees_.push_back (tree_); // Sort the clouds by their scales sort (scales_.begin (), scales_.end (), compareScalesFunction); // Find the index of the input after sorting for (size_t i = 0; i < scales_.size (); ++i) if (scales_[i].second == scales_.size () - 1) { input_index_ = i; break; } PCL_INFO ("Scales: "); for (size_t i = 0; i < scales_.size (); ++i) PCL_INFO ("(%d %f), ", scales_[i].second, scales_[i].first); PCL_INFO ("\n"); return (true); }
bool pcl::ProjectInliers<sensor_msgs::PointCloud2>::initSACModel (int model_type) { // Convert the input data PointCloud<PointXYZ> cloud; fromROSMsg (*input_, cloud); PointCloud<PointXYZ>::Ptr cloud_ptr = cloud.makeShared (); // Build the model switch (model_type) { case SACMODEL_PLANE: { //PCL_DEBUG ("[pcl::%s::initSACModel] Using a model of type: SACMODEL_PLANE\n", getClassName ().c_str ()); sacmodel_.reset (new SampleConsensusModelPlane<pcl::PointXYZ> (cloud_ptr)); break; } case SACMODEL_LINE: { //PCL_DEBUG ("[pcl::%s::initSACModel] Using a model of type: SACMODEL_LINE\n", getClassName ().c_str ()); sacmodel_.reset (new SampleConsensusModelLine<pcl::PointXYZ> (cloud_ptr)); break; } case SACMODEL_CIRCLE2D: { //PCL_DEBUG ("[pcl::%s::initSACModel] Using a model of type: SACMODEL_CIRCLE2D\n", getClassName ().c_str ()); sacmodel_.reset (new SampleConsensusModelCircle2D<pcl::PointXYZ> (cloud_ptr)); break; } case SACMODEL_SPHERE: { //PCL_DEBUG ("[pcl::%s::initSACModel] Using a model of type: SACMODEL_SPHERE\n", getClassName ().c_str ()); sacmodel_.reset (new SampleConsensusModelSphere<pcl::PointXYZ> (cloud_ptr)); break; } case SACMODEL_PARALLEL_LINE: { //PCL_DEBUG ("[pcl::%s::initSACModel] Using a model of type: SACMODEL_PARALLEL_LINE\n", getClassName ().c_str ()); sacmodel_.reset (new SampleConsensusModelParallelLine<pcl::PointXYZ> (cloud_ptr)); break; } case SACMODEL_PERPENDICULAR_PLANE: { //PCL_DEBUG ("[pcl::%s::initSACModel] Using a model of type: SACMODEL_PERPENDICULAR_PLANE\n", getClassName ().c_str ()); sacmodel_.reset (new SampleConsensusModelPerpendicularPlane<pcl::PointXYZ> (cloud_ptr)); break; } case SACMODEL_CYLINDER: { //PCL_DEBUG ("[pcl::%s::segment] Using a model of type: SACMODEL_CYLINDER\n", getClassName ().c_str ()); sacmodel_.reset (new SampleConsensusModelCylinder<pcl::PointXYZ, Normal> (cloud_ptr)); break; } case SACMODEL_NORMAL_PLANE: { //PCL_DEBUG ("[pcl::%s::segment] Using a model of type: SACMODEL_NORMAL_PLANE\n", getClassName ().c_str ()); sacmodel_.reset (new SampleConsensusModelNormalPlane<pcl::PointXYZ, Normal> (cloud_ptr)); break; } case SACMODEL_CONE: { //PCL_DEBUG ("[pcl::%s::segment] Using a model of type: SACMODEL_CONE\n", getClassName ().c_str ()); sacmodel_.reset (new SampleConsensusModelCone<pcl::PointXYZ, Normal> (cloud_ptr)); break; } case SACMODEL_NORMAL_SPHERE: { //PCL_DEBUG ("[pcl::%s::segment] Using a model of type: SACMODEL_NORMAL_SPHERE\n", getClassName ().c_str ()); sacmodel_.reset (new SampleConsensusModelNormalSphere<pcl::PointXYZ, Normal> (cloud_ptr)); break; } case SACMODEL_NORMAL_PARALLEL_PLANE: { //PCL_DEBUG ("[pcl::%s::segment] Using a model of type: SACMODEL_NORMAL_PARALLEL_PLANE\n", getClassName ().c_str ()); sacmodel_.reset (new SampleConsensusModelNormalParallelPlane<pcl::PointXYZ, Normal> (cloud_ptr)); break; } case SACMODEL_PARALLEL_PLANE: { //PCL_DEBUG ("[pcl::%s::segment] Using a model of type: SACMODEL_PARALLEL_PLANE\n", getClassName ().c_str ()); sacmodel_.reset (new SampleConsensusModelParallelPlane<pcl::PointXYZ> (cloud_ptr)); break; } default: { PCL_ERROR ("[pcl::%s::initSACModel] No valid model given!\n", getClassName ().c_str ()); return (false); } } return (true); }
template <typename PointSource, typename PointTarget, typename NormalT, typename Scalar> bool pcl::registration::FPCSInitialAlignment <PointSource, PointTarget, NormalT, Scalar>::initCompute () { std::srand (static_cast <unsigned int> (std::time (NULL))); // basic pcl initialization if (!pcl::PCLBase <PointSource>::initCompute ()) return (false); // check if source and target are given if (!input_ || !target_) { PCL_ERROR ("[%s::initCompute] Source or target dataset not given!\n", reg_name_.c_str ()); return (false); } if (!target_indices_ || target_indices_->size () == 0) { target_indices_.reset (new std::vector <int> (static_cast <int> (target_->size ()))); int index = 0; for (std::vector <int>::iterator it = target_indices_->begin (), it_e = target_indices_->end (); it != it_e; it++) *it = index++; target_cloud_updated_ = true; } // if a sample size for the point clouds is given; prefarably no sampling of target cloud if (nr_samples_ != 0) { const int ss = static_cast <int> (indices_->size ()); const int sample_fraction_src = std::max (1, static_cast <int> (ss / nr_samples_)); source_indices_ = pcl::IndicesPtr (new std::vector <int>); for (int i = 0; i < ss; i++) if (rand () % sample_fraction_src == 0) source_indices_->push_back ((*indices_) [i]); } else source_indices_ = indices_; // check usage of normals if (source_normals_ && target_normals_ && source_normals_->size () == input_->size () && target_normals_->size () == target_->size ()) use_normals_ = true; // set up tree structures if (target_cloud_updated_) { tree_->setInputCloud (target_, target_indices_); target_cloud_updated_ = false; } // set predefined variables const int min_iterations = 4; const float diameter_fraction = 0.3f; // get diameter of input cloud (distance between farthest points) Eigen::Vector4f pt_min, pt_max; pcl::getMinMax3D (*target_, *target_indices_, pt_min, pt_max); diameter_ = (pt_max - pt_min).norm (); // derive the limits for the random base selection float max_base_diameter = diameter_* approx_overlap_ * 2.f; max_base_diameter_sqr_ = max_base_diameter * max_base_diameter; // normalize the delta if (normalize_delta_) { float mean_dist = getMeanPointDensity <PointTarget> (target_, *target_indices_, 0.05f * diameter_, nr_threads_); delta_ *= mean_dist; } // heuristic determination of number of trials to have high probabilty of finding a good solution if (max_iterations_ == 0) { float first_est = std::log (small_error_) / std::log (1.0 - std::pow ((double) approx_overlap_, (double) min_iterations)); max_iterations_ = static_cast <int> (first_est / (diameter_fraction * approx_overlap_ * 2.f)); } // set further parameter if (score_threshold_ == FLT_MAX) score_threshold_ = 1.f - approx_overlap_; if (max_iterations_ < 4) max_iterations_ = 4; if (max_runtime_ < 1) max_runtime_ = INT_MAX; // calculate internal parameters based on the the estimated point density max_pair_diff_ = delta_ * 2.f; max_edge_diff_ = delta_ * 4.f; coincidation_limit_ = delta_ * 2.f; // EDITED: originally std::sqrt (delta_ * 2.f) max_mse_ = powf (delta_* 2.f, 2.f); max_inlier_dist_sqr_ = powf (delta_ * 2.f, 2.f); // reset fitness_score fitness_score_ = FLT_MAX; return (true); }
void pcl::ProjectInliers<sensor_msgs::PointCloud2>::applyFilter (PointCloud2 &output) { if (indices_->empty ()) { PCL_WARN ("[pcl::%s::applyFilter] No indices given or empty indices!\n", getClassName ().c_str ()); output.width = output.height = 0; output.data.clear (); return; } //Eigen::Map<Eigen::VectorXf, Eigen::Aligned> model_coefficients (&model_->values[0], model_->values.size ()); // More expensive than a map but safer (32bit architectures seem to complain) Eigen::VectorXf model_coefficients (model_->values.size ()); for (size_t i = 0; i < model_->values.size (); ++i) model_coefficients[i] = model_->values[i]; // Construct the model and project if (!initSACModel (model_type_)) { PCL_ERROR ("[pcl::%s::segment] Error initializing the SAC model!\n", getClassName ().c_str ()); output.width = output.height = 0; output.data.clear (); return; } pcl::PointCloud<pcl::PointXYZ> cloud_out; if (!copy_all_fields_) sacmodel_->projectPoints (*indices_, model_coefficients, cloud_out, false); else sacmodel_->projectPoints (*indices_, model_coefficients, cloud_out, true); if (copy_all_data_) { output.height = input_->height; output.width = input_->width; output.is_bigendian = input_->is_bigendian; output.point_step = input_->point_step; output.row_step = input_->row_step; output.data = input_->data; output.is_dense = input_->is_dense; // Get the distance field index int x_idx = -1, y_idx = -1, z_idx = -1; for (int d = 0; d < static_cast<int> (output.fields.size ()); ++d) { if (output.fields[d].name == "x") x_idx = d; if (output.fields[d].name == "y") y_idx = d; if (output.fields[d].name == "z") z_idx = d; } if (x_idx == -1 || y_idx == -1 || z_idx == -1) { PCL_ERROR ("[pcl::%s::segment] X (%d) Y (%d) Z (%d) field dimensions not found!\n", getClassName ().c_str (), x_idx, y_idx, z_idx); output.width = output.height = 0; output.data.clear (); return; } // Copy the projected points for (size_t i = 0; i < indices_->size (); ++i) { memcpy (&output.data[(*indices_)[i] * output.point_step + output.fields[x_idx].offset], &cloud_out.points[(*indices_)[i]].x, sizeof (float)); memcpy (&output.data[(*indices_)[i] * output.point_step + output.fields[y_idx].offset], &cloud_out.points[(*indices_)[i]].y, sizeof (float)); memcpy (&output.data[(*indices_)[i] * output.point_step + output.fields[z_idx].offset], &cloud_out.points[(*indices_)[i]].z, sizeof (float)); } } else { if (!copy_all_fields_) { pcl::toROSMsg<pcl::PointXYZ> (cloud_out, output); } else { // Copy everything output.height = 1; output.width = static_cast<uint32_t> (indices_->size ()); output.point_step = input_->point_step; output.data.resize (output.width * output.point_step); output.is_bigendian = input_->is_bigendian; output.row_step = output.point_step * output.width; // All projections should return valid data, so is_dense = true output.is_dense = true; // Get the distance field index int x_idx = -1, y_idx = -1, z_idx = -1; for (int d = 0; d < static_cast<int> (output.fields.size ()); ++d) { if (output.fields[d].name == "x") x_idx = d; if (output.fields[d].name == "y") y_idx = d; if (output.fields[d].name == "z") z_idx = d; } if (x_idx == -1 || y_idx == -1 || z_idx == -1) { PCL_ERROR ("[pcl::%s::segment] X (%d) Y (%d) Z (%d) field dimensions not found!\n", getClassName ().c_str (), x_idx, y_idx, z_idx); output.width = output.height = 0; output.data.clear (); return; } // Copy the projected points for (size_t i = 0; i < indices_->size (); ++i) { memcpy (&output.data[i * output.point_step], &input_->data[(*indices_)[i] * input_->point_step], output.point_step); memcpy (&output.data[i * output.point_step + output.fields[x_idx].offset], &cloud_out.points[(*indices_)[i]].x, sizeof (float)); memcpy (&output.data[i * output.point_step + output.fields[y_idx].offset], &cloud_out.points[(*indices_)[i]].y, sizeof (float)); memcpy (&output.data[i * output.point_step + output.fields[z_idx].offset], &cloud_out.points[(*indices_)[i]].z, sizeof (float)); } } } }
int pcl::io::saveVTKFile (const std::string &file_name, const pcl::PolygonMesh &triangles, unsigned precision) { if (triangles.cloud.data.empty ()) { PCL_ERROR ("[pcl::io::saveVTKFile] Input point cloud has no data!\n"); return (-1); } // Open file std::ofstream fs; fs.precision (precision); fs.open (file_name.c_str ()); unsigned int nr_points = triangles.cloud.width * triangles.cloud.height; unsigned int point_size = static_cast<unsigned int> (triangles.cloud.data.size () / nr_points); // Write the header information fs << "# vtk DataFile Version 3.0\nvtk output\nASCII\nDATASET POLYDATA\nPOINTS " << nr_points << " float" << std::endl; // Iterate through the points for (unsigned int i = 0; i < nr_points; ++i) { int xyz = 0; for (size_t d = 0; d < triangles.cloud.fields.size (); ++d) { int count = triangles.cloud.fields[d].count; if (count == 0) count = 1; // we simply cannot tolerate 0 counts (coming from older converter code) int c = 0; if ((triangles.cloud.fields[d].datatype == sensor_msgs::PointField::FLOAT32) && ( triangles.cloud.fields[d].name == "x" || triangles.cloud.fields[d].name == "y" || triangles.cloud.fields[d].name == "z")) { float value; memcpy (&value, &triangles.cloud.data[i * point_size + triangles.cloud.fields[d].offset + c * sizeof (float)], sizeof (float)); fs << value; if (++xyz == 3) break; } fs << " "; } if (xyz != 3) { PCL_ERROR ("[pcl::io::saveVTKFile] Input point cloud has no XYZ data!\n"); return (-2); } fs << std::endl; } // Write vertices fs << "\nVERTICES " << nr_points << " " << 2*nr_points << std::endl; for (unsigned int i = 0; i < nr_points; ++i) fs << "1 " << i << std::endl; // Write polygons // compute the correct number of values: size_t triangle_size = triangles.polygons.size (); size_t correct_number = triangle_size; for (size_t i = 0; i < triangle_size; ++i) correct_number += triangles.polygons[i].vertices.size (); fs << "\nPOLYGONS " << triangle_size << " " << correct_number << std::endl; for (size_t i = 0; i < triangle_size; ++i) { fs << triangles.polygons[i].vertices.size () << " "; size_t j = 0; for (j = 0; j < triangles.polygons[i].vertices.size () - 1; ++j) fs << triangles.polygons[i].vertices[j] << " "; fs << triangles.polygons[i].vertices[j] << std::endl; } // Write RGB values int field_index = getFieldIndex (triangles.cloud, "rgb"); if (field_index != -1) { fs << "\nPOINT_DATA " << nr_points << "\nCOLOR_SCALARS scalars 3\n"; for (unsigned int i = 0; i < nr_points; ++i) { int count = triangles.cloud.fields[field_index].count; if (count == 0) count = 1; // we simply cannot tolerate 0 counts (coming from older converter code) int c = 0; if (triangles.cloud.fields[field_index].datatype == sensor_msgs::PointField::FLOAT32) { pcl::RGB color; memcpy (&color, &triangles.cloud.data[i * point_size + triangles.cloud.fields[field_index].offset + c * sizeof (float)], sizeof (RGB)); int r = color.r; int g = color.g; int b = color.b; fs << static_cast<float> (r) / 255.0f << " " << static_cast<float> (g) / 255.0f << " " << static_cast<float> (b) / 255.0f; } fs << std::endl; } } // Close file fs.close (); return (0); }
template <typename PointFeature> bool pcl::PyramidFeatureHistogram<PointFeature>::initializeHistogram () { // a few consistency checks before starting the computations if (!PCLBase<PointFeature>::initCompute ()) { PCL_ERROR ("[pcl::PyramidFeatureHistogram::initializeHistogram] PCLBase initCompute failed\n"); return false; } if (dimension_range_input_.size () == 0) { PCL_ERROR ("[pcl::PyramidFeatureHistogram::initializeHistogram] Input dimension range was not set\n"); return false; } if (dimension_range_target_.size () == 0) { PCL_ERROR ("[pcl::PyramidFeatureHistogram::initializeHistogram] Target dimension range was not set\n"); return false; } if (dimension_range_input_.size () != dimension_range_target_.size ()) { PCL_ERROR ("[pcl::PyramidFeatureHistogram::initializeHistogram] Input and target dimension ranges do not agree in size: %u vs %u\n", dimension_range_input_.size (), dimension_range_target_.size ()); return false; } nr_dimensions = dimension_range_target_.size (); nr_features = input_->points.size (); float D = 0.0f; for (std::vector<std::pair<float, float> >::iterator range_it = dimension_range_target_.begin (); range_it != dimension_range_target_.end (); ++range_it) { float aux = range_it->first - range_it->second; D += aux * aux; } D = sqrt (D); nr_levels = ceil (Log2 (D)); PCL_DEBUG ("[pcl::PyramidFeatureHistogram::initializeHistogram] Pyramid will have %u levels with a hyper-parallelepiped diagonal size of %f\n", nr_levels, D); hist_levels.resize (nr_levels); for (size_t level_i = 0; level_i < nr_levels; ++level_i) { std::vector<size_t> bins_per_dimension (nr_dimensions); std::vector<float> bin_step (nr_dimensions); for (size_t dim_i = 0; dim_i < nr_dimensions; ++dim_i) { bins_per_dimension[dim_i] = ceil ( (dimension_range_target_[dim_i].second - dimension_range_target_[dim_i].first) / (pow (2.0f, (int) level_i) * sqrt ((float) nr_dimensions))); bin_step[dim_i] = pow (2.0f, (int) level_i) * sqrt ((float) nr_dimensions); } hist_levels[level_i] = PyramidFeatureHistogramLevel (bins_per_dimension, bin_step); PCL_DEBUG ("[pcl::PyramidFeatureHistogram::initializeHistogram] Created vector of size %u at level %u\nwith #bins per dimension:", hist_levels.back ().hist.size (), level_i); for (size_t dim_i = 0; dim_i < nr_dimensions; ++dim_i) PCL_DEBUG ("%u ", bins_per_dimension[dim_i]); PCL_DEBUG ("\n"); } return true; }
int main (int argc, char** argv) { std::string fileName = argv[1]; std::cout << "Reading " << fileName << std::endl; pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>); if (pcl::io::loadPCDFile<pcl::PointXYZ> (fileName, *cloud) == -1) //* load the file { PCL_ERROR ("Couldn't read file"); return (-1); } std::cout << "Loaded " << cloud->points.size() << " points." << std::endl; // Compute the normals pcl::NormalEstimation<pcl::PointXYZ, pcl::Normal> normalEstimation; normalEstimation.setInputCloud (cloud); pcl::search::KdTree<pcl::PointXYZ>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZ>); normalEstimation.setSearchMethod (tree); pcl::PointCloud<pcl::Normal>::Ptr cloudWithNormals (new pcl::PointCloud<pcl::Normal>); normalEstimation.setRadiusSearch (0.03); normalEstimation.compute (*cloudWithNormals); std::cout << "Computed " << cloudWithNormals->points.size() << " normals." << std::endl; // Setup the feature computation typedef pcl::VFHEstimation<pcl::PointXYZ, pcl::Normal, pcl::VFHSignature308> VFHEstimationType; VFHEstimationType vfhEstimation; // Provide the original point cloud (without normals) vfhEstimation.setInputCloud (cloud); // Provide the point cloud with normals vfhEstimation.setInputNormals(cloudWithNormals); // Use the same KdTree from the normal estimation vfhEstimation.setSearchMethod (tree); //vfhEstimation.setRadiusSearch (0.2); // With this, error: "Both radius (.2) and K (1) defined! Set one of them to zero first and then re-run compute()" // Actually compute the VFH features pcl::PointCloud<pcl::VFHSignature308>::Ptr vfhFeatures(new pcl::PointCloud<pcl::VFHSignature308>); vfhEstimation.compute (*vfhFeatures); std::cout << "output points.size (): " << vfhFeatures->points.size () << std::endl; // This outputs 1 - should be 397! // Display and retrieve the shape context descriptor vector for the 0th point. pcl::VFHSignature308 descriptor = vfhFeatures->points[0]; VFHEstimationType::PointCloudOut::PointType descriptor2 = vfhFeatures->points[0]; std::cout << "VFH:" << descriptor << std::endl; std::cout << "Numero de Elementos del VFH = " << sizeof(descriptor.histogram)/sizeof(descriptor.histogram[0]) << std::endl; // Create *_vfh.pcd file std::stringstream vfh_file; vfh_file << "# .PCD v.6 - Point Cloud Data file format" << std::endl; vfh_file << "FIELDS vfh" << std::endl; vfh_file << "SIZE 4" << std::endl; vfh_file << "TYPE F" << std::endl; vfh_file << "COUNT 308" << std::endl; vfh_file << "WIDTH 1" << std::endl; vfh_file << "HEIGHT 1" << std::endl; vfh_file << "POINTS 1" << std::endl; vfh_file << "DATA ascii" << std::endl; int vfh_length = sizeof(descriptor.histogram)/sizeof(descriptor.histogram[0]); for (int i = 0; i < vfh_length; i++) { vfh_file << descriptor.histogram[i] << " "; } std::ofstream outFile; outFile.open("Prueba_vfh.pcd"); outFile << vfh_file.str(); outFile.close(); return 0; }
template <typename PointFeature> float pcl::PyramidFeatureHistogram<PointFeature>::comparePyramidFeatureHistograms (const PyramidFeatureHistogramPtr &pyramid_a, const PyramidFeatureHistogramPtr &pyramid_b) { // do a few consistency checks before and during the computation if (pyramid_a->nr_dimensions != pyramid_b->nr_dimensions) { PCL_ERROR ("[pcl::PyramidFeatureMatching::comparePyramidFeatureHistograms] The two given pyramids have different numbers of dimensions: %u vs %u\n", pyramid_a->nr_dimensions, pyramid_b->nr_dimensions); return -1; } if (pyramid_a->nr_levels != pyramid_b->nr_levels) { PCL_ERROR ("[pcl::PyramidFeatureMatching::comparePyramidFeatureHistograms] The two given pyramids have different numbers of levels: %u vs %u\n", pyramid_a->nr_levels, pyramid_b->nr_levels); return -1; } // calculate for level 0 first if (pyramid_a->hist_levels[0].hist.size () != pyramid_b->hist_levels[0].hist.size ()) { PCL_ERROR ("[pcl::PyramidFeatureMatching::comparePyramidFeatureHistograms] The two given pyramids have different numbers of bins on level 0: %u vs %u\n", pyramid_a->hist_levels[0].hist.size (), pyramid_b->hist_levels[0].hist.size ()); return -1; } float match_count_level = 0.0f, match_count_prev_level = 0.0f; for (size_t bin_i = 0; bin_i < pyramid_a->hist_levels[0].hist.size (); ++bin_i) { if (pyramid_a->hist_levels[0].hist[bin_i] < pyramid_b->hist_levels[0].hist[bin_i]) match_count_level += pyramid_a->hist_levels[0].hist[bin_i]; else match_count_level += pyramid_b->hist_levels[0].hist[bin_i]; } float match_count = match_count_level; for (size_t level_i = 1; level_i < pyramid_a->nr_levels; ++level_i) { if (pyramid_a->hist_levels[level_i].hist.size () != pyramid_b->hist_levels[level_i].hist.size ()) { PCL_ERROR ("[pcl::PyramidFeatureMatching::comparePyramidFeatureHistograms] The two given pyramids have different numbers of bins on level %u: %u vs %u\n", level_i, pyramid_a->hist_levels[level_i].hist.size (), pyramid_b->hist_levels[level_i].hist.size ()); return -1; } match_count_prev_level = match_count_level; match_count_level = 0.0f; for (size_t bin_i = 0; bin_i < pyramid_a->hist_levels[level_i].hist.size (); ++bin_i) { if (pyramid_a->hist_levels[level_i].hist[bin_i] < pyramid_b->hist_levels[level_i].hist[bin_i]) match_count_level += pyramid_a->hist_levels[level_i].hist[bin_i]; else match_count_level += pyramid_b->hist_levels[level_i].hist[bin_i]; } float level_normalization_factor = pow(2.0f, (int) level_i); match_count += (match_count_level - match_count_prev_level) / level_normalization_factor; } // include self-similarity factors float self_similarity_a = pyramid_a->nr_features, self_similarity_b = pyramid_b->nr_features; PCL_DEBUG ("[pcl::PyramidFeatureMatching::comparePyramidFeatureHistograms] Self similarity measures: %f, %f\n", self_similarity_a, self_similarity_b); match_count /= sqrt (self_similarity_a * self_similarity_b); return match_count; }
template <typename PointInT, typename PointOutT, typename PointRFT> bool pcl::UniqueShapeContext<PointInT, PointOutT, PointRFT>::initCompute () { if (!Feature<PointInT, PointOutT>::initCompute ()) { PCL_ERROR ("[pcl::%s::initCompute] Init failed.\n", getClassName ().c_str ()); return (false); } // Default LRF estimation alg: SHOTLocalReferenceFrameEstimation typename SHOTLocalReferenceFrameEstimation<PointInT, PointRFT>::Ptr lrf_estimator(new SHOTLocalReferenceFrameEstimation<PointInT, PointRFT>()); lrf_estimator->setRadiusSearch (local_radius_); lrf_estimator->setInputCloud (input_); lrf_estimator->setIndices (indices_); if (!fake_surface_) lrf_estimator->setSearchSurface(surface_); if (!FeatureWithLocalReferenceFrames<PointInT, PointRFT>::initLocalReferenceFrames (indices_->size (), lrf_estimator)) { PCL_ERROR ("[pcl::%s::initCompute] Init failed.\n", getClassName ().c_str ()); return (false); } if (search_radius_< min_radius_) { PCL_ERROR ("[pcl::%s::initCompute] search_radius_ must be GREATER than min_radius_.\n", getClassName ().c_str ()); return (false); } // Update descriptor length descriptor_length_ = elevation_bins_ * azimuth_bins_ * radius_bins_; // Compute radial, elevation and azimuth divisions float azimuth_interval = 360.0f / static_cast<float> (azimuth_bins_); float elevation_interval = 180.0f / static_cast<float> (elevation_bins_); // Reallocate divisions and volume lut radii_interval_.clear (); phi_divisions_.clear (); theta_divisions_.clear (); volume_lut_.clear (); // Fills radii interval based on formula (1) in section 2.1 of Frome's paper radii_interval_.resize (radius_bins_ + 1); for (size_t j = 0; j < radius_bins_ + 1; j++) radii_interval_[j] = static_cast<float> (exp (log (min_radius_) + ((static_cast<float> (j) / static_cast<float> (radius_bins_)) * log (search_radius_/min_radius_)))); // Fill theta didvisions of elevation theta_divisions_.resize (elevation_bins_+1); for (size_t k = 0; k < elevation_bins_+1; k++) theta_divisions_[k] = static_cast<float> (k) * elevation_interval; // Fill phi didvisions of elevation phi_divisions_.resize (azimuth_bins_+1); for (size_t l = 0; l < azimuth_bins_+1; l++) phi_divisions_[l] = static_cast<float> (l) * azimuth_interval; // LookUp Table that contains the volume of all the bins // "phi" term of the volume integral // "integr_phi" has always the same value so we compute it only one time float integr_phi = pcl::deg2rad (phi_divisions_[1]) - pcl::deg2rad (phi_divisions_[0]); // exponential to compute the cube root using pow float e = 1.0f / 3.0f; // Resize volume look up table volume_lut_.resize (radius_bins_ * elevation_bins_ * azimuth_bins_); // Fill volumes look up table for (size_t j = 0; j < radius_bins_; j++) { // "r" term of the volume integral float integr_r = (radii_interval_[j+1]*radii_interval_[j+1]*radii_interval_[j+1] / 3) - (radii_interval_[j]*radii_interval_[j]*radii_interval_[j]/ 3); for (size_t k = 0; k < elevation_bins_; k++) { // "theta" term of the volume integral float integr_theta = cosf (deg2rad (theta_divisions_[k])) - cosf (deg2rad (theta_divisions_[k+1])); // Volume float V = integr_phi * integr_theta * integr_r; // Compute cube root of the computed volume commented for performance but left // here for clarity // float cbrt = pow(V, e); // cbrt = 1 / cbrt; for (size_t l = 0; l < azimuth_bins_; l++) // Store in lut 1/cbrt //volume_lut_[ (l*elevation_bins_*radius_bins_) + k*radius_bins_ + j ] = cbrt; volume_lut_[(l*elevation_bins_*radius_bins_) + k*radius_bins_ + j] = 1.0f / powf (V, e); } } return (true); }
void PcdGtAnnotator<PointT>::annotate (const std::string &scenes_dir, const std::string &scene_id) { std::string scene_full_path = scenes_dir + "/" + scene_id; std::vector < std::string > scene_view; if (v4r::io::getFilesInDirectory( scene_full_path, scene_view, "", ".*.pcd", true) != -1) { std::cout << "Number of viewpoints in directory is:" << scene_view.size () << std::endl; std::vector < std::string > gt_file; std::string annotations_dir = gt_dir_ + "/" + scene_id; if( v4r::io::getFilesInDirectory( annotations_dir, gt_file, "", ".*.txt", true) == -1) { std::cerr << "Could not find any annotations in " << annotations_dir << ". " << std::endl; } std::sort(gt_file.begin(), gt_file.end()); std::sort(scene_view.begin(), scene_view.end()); for (size_t s_id = 0; s_id < scene_view.size (); s_id++) { if (first_view_only_ && s_id > 0) continue; std::vector<std::string> strs; boost::split (strs, scene_view[s_id], boost::is_any_of (".")); std::string scene_file_wo_ext = strs[0] + "_"; std::string gt_occ_check = scene_file_wo_ext + "occlusion_"; std::string scene_full_file_path = scene_full_path + "/" + scene_view[s_id]; pcl::io::loadPCDFile(scene_full_file_path, *reconstructed_scene_); View view; pcl::copyPointCloud(*reconstructed_scene_, *view.cloud_); view.cloud_->sensor_origin_ = reconstructed_scene_->sensor_origin_; view.cloud_->sensor_orientation_ = reconstructed_scene_->sensor_orientation_; for(size_t gt_id=0; gt_id < gt_file.size(); gt_id++) { const std::string gt_fn = gt_file[gt_id]; if( gt_fn.compare(0, scene_file_wo_ext.size(), scene_file_wo_ext) == 0 && gt_fn.compare(0, gt_occ_check.size(), gt_occ_check)) { std::cout << gt_fn << std::endl; std::string model_instance = gt_fn.substr(scene_file_wo_ext.size()); std::vector<std::string> str_split; boost::split (str_split, model_instance, boost::is_any_of (".")); model_instance = str_split[0]; size_t found = model_instance.find_last_of("_"); std::string times_text ("times.txt"); if ( model_instance.compare(times_text) == 0 ) { std::cout << "skipping this one" << std::endl; continue; } std::string model_name = model_instance.substr(0,found) + ".pcd"; std::cout << "Model: " << model_name << std::endl; ModelTPtr pModel; source_->getModelById(model_name, pModel); std::string gt_full_file_path = annotations_dir + "/" + gt_fn; Eigen::Matrix4f transform = v4r::io::readMatrixFromFile(gt_full_file_path); typename pcl::PointCloud<PointT>::ConstPtr model_cloud = pModel->getAssembled(0.003f); typename pcl::PointCloud<PointT>::Ptr model_aligned(new pcl::PointCloud<PointT>()); pcl::transformPointCloud(*model_cloud, *model_aligned, transform); bool model_exists = false; size_t current_model_id = 0; for (size_t m_id=0; m_id < model_id_.size(); m_id++) { if ( model_id_[m_id].compare( model_instance ) == 0) { model_exists = true; current_model_id = m_id; transform_to_scene_[m_id] = transform; break; } } if ( !model_exists ) { std::vector<bool> visible_model_pts_tmp (model_aligned->points.size(), false); visible_model_points_.push_back(visible_model_pts_tmp); model_id_.push_back( model_instance ); transform_to_scene_.push_back(transform); std::vector<bool> obj_mask (reconstructed_scene_->points.size(), false); pixel_annotated_obj_in_first_view_.push_back ( obj_mask ); current_model_id = model_id_.size()-1; } const float cx = (static_cast<float> (reconstructed_scene_->width) / 2.f - 0.5f); const float cy = (static_cast<float> (reconstructed_scene_->height) / 2.f - 0.5f); for(size_t m_pt_id=0; m_pt_id < model_aligned->points.size(); m_pt_id++) { PointT mp = model_aligned->points[m_pt_id]; const int u = static_cast<int> (f_ * mp.x / mp.z + cx); const int v = static_cast<int> (f_ * mp.y / mp.z + cy); if(u<0 || u >= (int)reconstructed_scene_->width || v<0 || v >= (int)reconstructed_scene_->height) // model point outside field of view continue; PointT sp = reconstructed_scene_->at(u,v); if ( !pcl::isFinite(sp) ) // Model point is not visible from the view point (shiny spot, noise,...) continue; if( std::abs(mp.z - sp.z) < threshold_ ) { visible_model_points_[current_model_id][m_pt_id] = true; if( s_id == 0 ) pixel_annotated_obj_in_first_view_[current_model_id][v*reconstructed_scene_->width + u] = true; } } } } std::cout << std::endl; // if(s_id==0) // { // pcl::visualization::PCLVisualizer vis("obj_mask"); // vis.addPointCloud(pScenePCl_, "cloud"); // for (size_t m_id=0; m_id < model_id_.size(); m_id++) // { // typename pcl::PointCloud<PointT>::Ptr highlight (new pcl::PointCloud<PointT>()); // pcl::copyPointCloud(*pScenePCl_, pixel_annotated_obj_in_first_view_[m_id], *highlight); // const float r=50+rand()%205; // const float g=50+rand()%205; // const float b=50+rand()%205; // std::stringstream model_name; // model_name << m_id; // pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZRGB> color (highlight, r, g, b); // vis.addPointCloud(highlight, color, model_name.str()); // vis.addText(model_name.str(), 40, 15*m_id, 15, r/255, g/255, b/255,model_name.str()); // } // vis.spin(); // } view.transform_to_scene_ = transform_to_scene_; views_.push_back(view); } // visualize(); } else { PCL_ERROR("You should pass a directory\n"); return; } }
template <typename PointT, typename PointNT> bool pcl::SampleConsensusModelCylinder<PointT, PointNT>::computeModelCoefficients ( const std::vector<int> &samples, Eigen::VectorXf &model_coefficients) { // Need 2 samples if (samples.size () != 2) { PCL_ERROR ("[pcl::SampleConsensusModelCylinder::computeModelCoefficients] Invalid set of samples given (%zu)!\n", samples.size ()); return (false); } if (!normals_) { PCL_ERROR ("[pcl::SampleConsensusModelCylinder::computeModelCoefficients] No input dataset containing normals was given!\n"); return (false); } if (fabs (input_->points[samples[0]].x - input_->points[samples[1]].x) <= std::numeric_limits<float>::epsilon () && fabs (input_->points[samples[0]].y - input_->points[samples[1]].y) <= std::numeric_limits<float>::epsilon () && fabs (input_->points[samples[0]].z - input_->points[samples[1]].z) <= std::numeric_limits<float>::epsilon ()) { return (false); } Eigen::Vector4f p1 (input_->points[samples[0]].x, input_->points[samples[0]].y, input_->points[samples[0]].z, 0); Eigen::Vector4f p2 (input_->points[samples[1]].x, input_->points[samples[1]].y, input_->points[samples[1]].z, 0); Eigen::Vector4f n1 (normals_->points[samples[0]].normal[0], normals_->points[samples[0]].normal[1], normals_->points[samples[0]].normal[2], 0); Eigen::Vector4f n2 (normals_->points[samples[1]].normal[0], normals_->points[samples[1]].normal[1], normals_->points[samples[1]].normal[2], 0); Eigen::Vector4f w = n1 + p1 - p2; float a = n1.dot (n1); float b = n1.dot (n2); float c = n2.dot (n2); float d = n1.dot (w); float e = n2.dot (w); float denominator = a*c - b*b; float sc, tc; // Compute the line parameters of the two closest points if (denominator < 1e-8) // The lines are almost parallel { sc = 0.0f; tc = (b > c ? d / b : e / c); // Use the largest denominator } else { sc = (b*e - c*d) / denominator; tc = (a*e - b*d) / denominator; } // point_on_axis, axis_direction Eigen::Vector4f line_pt = p1 + n1 + sc * n1; Eigen::Vector4f line_dir = p2 + tc * n2 - line_pt; line_dir.normalize (); model_coefficients.resize (7); // model_coefficients.template head<3> () = line_pt.template head<3> (); model_coefficients[0] = line_pt[0]; model_coefficients[1] = line_pt[1]; model_coefficients[2] = line_pt[2]; // model_coefficients.template segment<3> (3) = line_dir.template head<3> (); model_coefficients[3] = line_dir[0]; model_coefficients[4] = line_dir[1]; model_coefficients[5] = line_dir[2]; // cylinder radius model_coefficients[6] = static_cast<float> (sqrt (pcl::sqrPointToLineDistance (p1, line_pt, line_dir))); if (model_coefficients[6] > radius_max_ || model_coefficients[6] < radius_min_) return (false); return (true); }
bool pcl::concatenatePointCloud (const pcl::PCLPointCloud2 &cloud1, const pcl::PCLPointCloud2 &cloud2, pcl::PCLPointCloud2 &cloud_out) { //if one input cloud has no points, but the other input does, just return the cloud with points if (cloud1.width*cloud1.height == 0 && cloud2.width*cloud2.height > 0) { cloud_out = cloud2; return (true); } else if (cloud1.width*cloud1.height > 0 && cloud2.width*cloud2.height == 0) { cloud_out = cloud1; return (true); } bool strip = false; for (size_t i = 0; i < cloud1.fields.size (); ++i) if (cloud1.fields[i].name == "_") strip = true; for (size_t i = 0; i < cloud2.fields.size (); ++i) if (cloud2.fields[i].name == "_") strip = true; if (!strip && cloud1.fields.size () != cloud2.fields.size ()) { PCL_ERROR ("[pcl::concatenatePointCloud] Number of fields in cloud1 (%u) != Number of fields in cloud2 (%u)\n", cloud1.fields.size (), cloud2.fields.size ()); return (false); } // Copy cloud1 into cloud_out cloud_out = cloud1; size_t nrpts = cloud_out.data.size (); // Height = 1 => no more organized cloud_out.width = cloud1.width * cloud1.height + cloud2.width * cloud2.height; cloud_out.height = 1; if (!cloud1.is_dense || !cloud2.is_dense) cloud_out.is_dense = false; else cloud_out.is_dense = true; // We need to strip the extra padding fields if (strip) { // Get the field sizes for the second cloud std::vector<pcl::PCLPointField> fields2; std::vector<int> fields2_sizes; for (size_t j = 0; j < cloud2.fields.size (); ++j) { if (cloud2.fields[j].name == "_") continue; fields2_sizes.push_back (cloud2.fields[j].count * pcl::getFieldSize (cloud2.fields[j].datatype)); fields2.push_back (cloud2.fields[j]); } cloud_out.data.resize (nrpts + (cloud2.width * cloud2.height) * cloud_out.point_step); // Copy the second cloud for (size_t cp = 0; cp < cloud2.width * cloud2.height; ++cp) { int i = 0; for (size_t j = 0; j < fields2.size (); ++j) { if (cloud1.fields[i].name == "_") { ++i; continue; } // We're fine with the special RGB vs RGBA use case if ((cloud1.fields[i].name == "rgb" && fields2[j].name == "rgba") || (cloud1.fields[i].name == "rgba" && fields2[j].name == "rgb") || (cloud1.fields[i].name == fields2[j].name)) { memcpy (reinterpret_cast<char*> (&cloud_out.data[nrpts + cp * cloud1.point_step + cloud1.fields[i].offset]), reinterpret_cast<const char*> (&cloud2.data[cp * cloud2.point_step + cloud2.fields[j].offset]), fields2_sizes[j]); ++i; // increment the field size i } } } } else { for (size_t i = 0; i < cloud1.fields.size (); ++i) { // We're fine with the special RGB vs RGBA use case if ((cloud1.fields[i].name == "rgb" && cloud2.fields[i].name == "rgba") || (cloud1.fields[i].name == "rgba" && cloud2.fields[i].name == "rgb")) continue; // Otherwise we need to make sure the names are the same if (cloud1.fields[i].name != cloud2.fields[i].name) { PCL_ERROR ("[pcl::concatenatePointCloud] Name of field %d in cloud1, %s, does not match name in cloud2, %s\n", i, cloud1.fields[i].name.c_str (), cloud2.fields[i].name.c_str ()); return (false); } } cloud_out.data.resize (nrpts + cloud2.data.size ()); memcpy (&cloud_out.data[nrpts], &cloud2.data[0], cloud2.data.size ()); } return (true); }