template <typename PointT> bool pcl::PackedRGBComparison<PointT>::evaluate (const PointT &point) const { // extract the component value uint8_t* pt_data = (uint8_t*)&point; uint8_t my_val = *(pt_data + component_offset_); // now do the comparison switch (this->op_) { case pcl::ComparisonOps::GT : return (my_val > this->compare_val_); case pcl::ComparisonOps::GE : return (my_val >= this->compare_val_); case pcl::ComparisonOps::LT : return (my_val < this->compare_val_); case pcl::ComparisonOps::LE : return (my_val <= this->compare_val_); case pcl::ComparisonOps::EQ : return (my_val == this->compare_val_); default: PCL_WARN ("[pcl::PackedRGBComparison::evaluate] unrecognized op_!\n"); return (false); } }
template <typename GeneratorT> int pcl::common::CloudGenerator<pcl::PointXY, GeneratorT>::fill (int width, int height, pcl::PointCloud<pcl::PointXY>& cloud) { if (width < 1) { PCL_ERROR ("[pcl::common::CloudGenerator] Cloud width must be >= 1\n!"); return (-1); } if (height < 1) { PCL_ERROR ("[pcl::common::CloudGenerator] Cloud height must be >= 1\n!"); return (-1); } if (!cloud.empty ()) PCL_WARN ("[pcl::common::CloudGenerator] Cloud data will be erased with new data\n!"); cloud.width = width; cloud.height = height; cloud.resize (cloud.width * cloud.height); cloud.is_dense = true; for (pcl::PointCloud<pcl::PointXY>::iterator points_it = cloud.begin (); points_it != cloud.end (); ++points_it) { points_it->x = x_generator_.run (); points_it->y = y_generator_.run (); } return (0); }
pcl::FeatureHistogram::FeatureHistogram (size_t const number_of_bins, const float min, const float max) : histogram_ (number_of_bins, 0) { // Initialize thresholds. if (min < max) { threshold_min_ = min; threshold_max_ = max; step_ = (max - min) / static_cast<float> (number_of_bins_); } else { threshold_min_ = 0.0f; threshold_max_ = static_cast<float> (number_of_bins); step_ = 1.0f; PCL_WARN ("[FeatureHistogram::setThresholds] Variable \"max\" must be greater then \"min\".\n"); } // Initialize sum. number_of_elements_ = 0; // Initialize size; number_of_bins_ = number_of_bins; }
void SACNormalsPlaneExtractor<PointT>::compute() { CHECK ( cloud_ ) << "Input cloud is not organized!"; all_planes_.clear(); // ---[ PassThroughFilter typename pcl::PointCloud<PointT>::Ptr cloud_filtered (new pcl::PointCloud<PointT> ()); pcl::PassThrough<PointT> pass; pass.setFilterLimits (0, max_z_bounds_); pass.setFilterFieldName ("z"); pass.setInputCloud (cloud_); pass.filter (*cloud_filtered); if ( cloud_filtered->points.size () < k_) { PCL_WARN ("[DominantPlaneSegmentation] Filtering returned %lu points! Aborting.", cloud_filtered->points.size ()); return; } // Downsample the point cloud typename pcl::PointCloud<PointT>::Ptr cloud_downsampled (new pcl::PointCloud<PointT> ()); pcl::VoxelGrid<PointT> grid; grid.setLeafSize (downsample_leaf_, downsample_leaf_, downsample_leaf_); grid.setDownsampleAllData (false); grid.setInputCloud (cloud_filtered); grid.filter (*cloud_downsampled); // ---[ Estimate the point normals pcl::PointCloud<pcl::Normal>::Ptr cloud_normals (new pcl::PointCloud<pcl::Normal> ()); pcl::NormalEstimation<PointT, pcl::Normal> n3d; typename pcl::search::KdTree<PointT>::Ptr normals_tree_ (new pcl::search::KdTree<PointT>); n3d.setKSearch ( (int) k_); n3d.setSearchMethod (normals_tree_); n3d.setInputCloud (cloud_downsampled); n3d.compute (*cloud_normals); // ---[ Perform segmentation pcl::SACSegmentationFromNormals<PointT, pcl::Normal> seg; seg.setDistanceThreshold (sac_distance_threshold_); seg.setMaxIterations (2000); seg.setNormalDistanceWeight (0.1); seg.setOptimizeCoefficients (true); seg.setModelType (pcl::SACMODEL_NORMAL_PLANE); seg.setMethodType (pcl::SAC_RANSAC); seg.setProbability (0.99); seg.setInputCloud (cloud_downsampled); seg.setInputNormals (cloud_normals); pcl::PointIndices table_inliers; pcl::ModelCoefficients coefficients; seg.segment ( table_inliers, coefficients); Eigen::Vector4f plane = Eigen::Vector4f(coefficients.values[0], coefficients.values[1], coefficients.values[2], coefficients.values[3]); all_planes_.resize(1); all_planes_[0] = plane; }
void ScreenshotManager::saveImage(const Eigen::Affine3f &camPose, pcl::gpu::PtrStepSz<const PixelRGB> rgb24) { PCL_WARN ("[o] [o] [o] [o] Saving screenshot [o] [o] [o] [o]\n"); std::string file_extension_image = ".png"; std::string file_extension_pose = ".txt"; std::string filename_image = "KinFuSnapshots/"; std::string filename_pose = "KinFuSnapshots/"; // Get Pose Eigen::Matrix<float, 3, 3, Eigen::RowMajor> erreMats = camPose.linear (); Eigen::Vector3f teVecs = camPose.translation (); // Create filenames filename_pose += std::to_string(screenshot_counter) + file_extension_pose; filename_image += std::to_string(screenshot_counter) + file_extension_image; // Write files writePose (filename_pose, teVecs, erreMats); // Save Image pcl::io::saveRgbPNGFile (filename_image, (unsigned char*)rgb24.data, 640,480); screenshot_counter++; }
/** @brief Asks the user to press enter to continue * @param[in] str Message to display */ void waitForUser (std::string str = "Press enter to continue") { PCL_WARN (str.c_str ()); std::cout.flush (); getc (stdin); }
template <typename PointT> bool pcl::visualization::PCLHistogramVisualizer::updateFeatureHistogram ( const pcl::PointCloud<PointT> &cloud, int hsize, const std::string &id) { RenWinInteractMap::iterator am_it = wins_.find (id); if (am_it == wins_.end ()) { PCL_WARN ("[updateFeatureHistogram] A window with id <%s> does not exists!.\n", id.c_str ()); return (false); } RenWinInteract* renwinupd = &wins_[id]; vtkSmartPointer<vtkDoubleArray> xy_array = vtkSmartPointer<vtkDoubleArray>::New (); xy_array->SetNumberOfComponents (2); xy_array->SetNumberOfTuples (hsize); // Parse the cloud data and store it in the array double xy[2]; for (int d = 0; d < hsize; ++d) { xy[0] = d; xy[1] = cloud.points[0].histogram[d]; xy_array->SetTuple (d, xy); } reCreateActor (xy_array, renwinupd, hsize); return (true); }
/** @brief Main function * @param argc * @param argv * @return Exit status */ int main (int argc, char *argv[]) { if (argc != 2) { PCL_ERROR ("Usage:\n%s 192.168.100.65\n", argv[0]); return (-1); } davidsdk_ptr.reset (new pcl::DavidSDKGrabber); davidsdk_ptr->connect (argv[1]); if (!davidsdk_ptr->isConnected ()) return (-1); PCL_WARN ("davidSDK connected\n"); boost::function<void (const boost::shared_ptr<pcl::PCLImage> &)> f = boost::bind (&grabberCallback, _1); davidsdk_ptr->registerCallback (f); davidsdk_ptr->start (); waitForUser ("Press enter to quit"); return (0); }
template<typename PointType, typename PointRfType> void pcl::Hough3DGrouping<PointModelT, PointSceneT, PointModelRfT, PointSceneRfT>::computeRf (const boost::shared_ptr<const pcl::PointCloud<PointType> > &input, pcl::PointCloud<PointRfType> &rf) { if (local_rf_search_radius_ == 0) { PCL_WARN ("[pcl::Hough3DGrouping::computeRf()] Warning! Reference frame search radius not set. Computing with default value. Results might be incorrect, algorithm might be slow.\n"); local_rf_search_radius_ = static_cast<float> (hough_bin_size_); } pcl::PointCloud<Normal>::Ptr normal_cloud (new pcl::PointCloud<Normal> ()); NormalEstimation<PointType, Normal> norm_est; norm_est.setInputCloud (input); if (local_rf_normals_search_radius_ <= 0.0f) { norm_est.setKSearch (15); } else { norm_est.setRadiusSearch (local_rf_normals_search_radius_); } norm_est.compute (*normal_cloud); BOARDLocalReferenceFrameEstimation<PointType, Normal, PointRfType> rf_est; rf_est.setInputCloud (input); rf_est.setInputNormals (normal_cloud); rf_est.setFindHoles (true); rf_est.setRadiusSearch (local_rf_search_radius_); rf_est.compute (rf); }
void computeFacesImpl <pcl::PointXYZ, pcl::PointXYZ> (v4r::Model<pcl::PointXYZ> & model) { (void) model; PCL_WARN("Not implemented for pcl::PointXYZ... this function would be available for PCL1.7.2 or higher\n"); }
template <typename PointT> void pcl::SupervoxelClustering<PointT>::reseedSupervoxels () { //Go through each supervoxel and remove all it's leaves for (typename HelperListT::iterator sv_itr = supervoxel_helpers_.begin (); sv_itr != supervoxel_helpers_.end (); ++sv_itr) { sv_itr->removeAllLeaves (); } std::vector<int> closest_index; std::vector<float> distance; //Now go through each supervoxel, find voxel closest to its center, add it in for (typename HelperListT::iterator sv_itr = supervoxel_helpers_.begin (); sv_itr != supervoxel_helpers_.end (); ++sv_itr) { PointT point; sv_itr->getXYZ (point.x, point.y, point.z); voxel_kdtree_->nearestKSearch (point, 1, closest_index, distance); LeafContainerT* seed_leaf = adjacency_octree_->at (closest_index[0]); if (seed_leaf) { sv_itr->addLeaf (seed_leaf); } else { PCL_WARN ("Could not find leaf in pcl::SupervoxelClustering<PointT>::reseedSupervoxels - supervoxel will be deleted \n"); } } }
void setCylinderModel(const Cylinder &cylinderModel){ cylinderModel_.color=cylinderModel.color; cylinderModel_.diameter=cylinderModel.diameter; cylinderModel_.height=cylinderModel.height; cylinderModelSet_=true; if (debug_) PCL_WARN("Set cylinder model to diameter: %f, height: %f\n",cylinderModel_.diameter,cylinderModel_.height); }
template <typename PointT> bool pcl::visualization::PCLHistogramVisualizer::addFeatureHistogram ( const pcl::PointCloud<PointT> &cloud, int hsize, const std::string &id, int win_width, int win_height) { RenWinInteractMap::iterator am_it = wins_.find (id); if (am_it != wins_.end ()) { PCL_WARN ("[addFeatureHistogram] A window with id <%s> already exists! Please choose a different id and retry.\n", id.c_str ()); return (false); } vtkSmartPointer<vtkDoubleArray> xy_array = vtkSmartPointer<vtkDoubleArray>::New (); xy_array->SetNumberOfComponents (2); xy_array->SetNumberOfTuples (hsize); // Parse the cloud data and store it in the array double xy[2]; for (int d = 0; d < hsize; ++d) { xy[0] = d; xy[1] = cloud.points[0].histogram[d]; xy_array->SetTuple (d, xy); } RenWinInteract renwinint; createActor (xy_array, renwinint, id, win_width, win_height); // Save the pointer/ID pair to the global window map wins_[id] = renwinint; resetStoppedFlag (); return (true); }
template <typename PointT> void pcl::ProjectInliers<PointT>::applyFilter (PointCloud &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.points.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]; // Initialize the Sample Consensus model and set its parameters if (!initSACModel (model_type_)) { PCL_ERROR ("[pcl::%s::segment] Error initializing the SAC model!\n", getClassName ().c_str ()); output.width = output.height = 0; output.points.clear (); return; } if (copy_all_data_) sacmodel_->projectPoints (*indices_, model_coefficients, output, true); else sacmodel_->projectPoints (*indices_, model_coefficients, output, false); }
void pcl::ImageGrabberBase::ImageGrabberImpl::loadDepthAndRGBFiles (const std::string &depth_dir, const std::string &rgb_dir) { if (!boost::filesystem::exists (depth_dir) || !boost::filesystem::is_directory (depth_dir)) { PCL_ERROR ("[pcl::ImageGrabber::loadDepthAndRGBFiles] Error: attempted to instantiate a pcl::ImageGrabber from a path which" " is not a directory: %s", depth_dir.c_str ()); return; } if (!boost::filesystem::exists (rgb_dir) || !boost::filesystem::is_directory (rgb_dir)) { PCL_ERROR ("[pcl::ImageGrabber::loadDepthAndRGBFiles] Error: attempted to instantiate a pcl::ImageGrabber from a path which" " is not a directory: %s", rgb_dir.c_str ()); return; } std::string pathname; std::string extension; std::string basename; boost::filesystem::directory_iterator end_itr; // First iterate over depth images for (boost::filesystem::directory_iterator itr (depth_dir); itr != end_itr; ++itr) { extension = boost::algorithm::to_upper_copy (boost::filesystem::extension (itr->path ())); pathname = itr->path ().string (); basename = boost::filesystem::basename (itr->path ()); if (!boost::filesystem::is_directory (itr->status ()) && isValidExtension (extension)) { if (basename.find ("depth") < std::string::npos) { depth_image_files_.push_back (pathname); } } } // Then iterate over RGB images for (boost::filesystem::directory_iterator itr (rgb_dir); itr != end_itr; ++itr) { extension = boost::algorithm::to_upper_copy (boost::filesystem::extension (itr->path ())); pathname = itr->path ().string (); basename = boost::filesystem::basename (itr->path ()); if (!boost::filesystem::is_directory (itr->status ()) && isValidExtension (extension)) { if (basename.find ("rgb") < std::string::npos) { rgb_image_files_.push_back (pathname); } } } if (depth_image_files_.size () != rgb_image_files_.size () ) PCL_WARN ("[pcl::ImageGrabberBase::ImageGrabberImpl::loadDepthAndRGBFiles] : Watch out not same amount of depth and rgb images"); if (!depth_image_files_.empty ()) sort (depth_image_files_.begin (), depth_image_files_.end ()); else PCL_ERROR ("[pcl::ImageGrabberBase::ImageGrabberImpl::loadDepthAndRGBFiles] : no depth images added"); if (!rgb_image_files_.empty ()) sort (rgb_image_files_.begin (), rgb_image_files_.end ()); else PCL_ERROR ("[pcl::ImageGrabberBase::ImageGrabberImpl::loadDepthAndRGBFiles] : no rgb images added"); }
void ctrlC (int) { boost::mutex::scoped_lock io_lock (io_mutex); std::cout << std::endl; PCL_WARN ("Ctrl-C detected, exit condition set to true\n"); is_done = true; }
template <typename PointT> bool PCLVisualizer::addCorrespondences ( const typename pcl::PointCloud<PointT>::ConstPtr &source_points, const typename pcl::PointCloud<PointT>::ConstPtr &target_points, const std::vector<pcl::Correspondence> &correspondences, const std::string &id, int viewport) { // Check to see if this ID entry already exists (has it been already added to the visualizer?) ShapeActorMap::iterator am_it = shape_actor_map_->find (id); if (am_it != shape_actor_map_->end ()) { PCL_WARN ("[addCorrespondences] A set of correspondences with id <%s> already exists! Please choose a different id and retry.\n", id.c_str ()); return (false); } vtkSmartPointer<vtkAppendPolyData> polydata = vtkSmartPointer<vtkAppendPolyData>::New (); vtkSmartPointer<vtkUnsignedCharArray> line_colors = vtkSmartPointer<vtkUnsignedCharArray>::New (); line_colors->SetNumberOfComponents (3); line_colors->SetName ("Colors"); unsigned char rgb[3]; // Use Red by default (can be changed later) rgb[0] = 1 * 255.0; rgb[1] = 0 * 255.0; rgb[2] = 0 * 255.0; // Draw lines between the best corresponding points for (size_t i = 0; i < correspondences.size (); ++i) { const PointT &p_src = source_points->points[correspondences[i].index_query]; const PointT &p_tgt = target_points->points[correspondences[i].index_match]; // Add the line vtkSmartPointer<vtkLineSource> line = vtkSmartPointer<vtkLineSource>::New (); line->SetPoint1 (p_src.x, p_src.y, p_src.z); line->SetPoint2 (p_tgt.x, p_tgt.y, p_tgt.z); line->Update (); polydata->AddInput (line->GetOutput ()); line_colors->InsertNextTupleValue (rgb); } polydata->Update (); vtkSmartPointer<vtkPolyData> line_data = polydata->GetOutput (); line_data->GetCellData ()->SetScalars (line_colors); // Create an Actor vtkSmartPointer<vtkLODActor> actor; createActorFromVTKDataSet (line_data, actor); actor->GetProperty ()->SetRepresentationToWireframe (); actor->GetProperty ()->SetOpacity (0.5); addActorToRenderer (actor, viewport); // Save the pointer/ID pair to the global actor map (*shape_actor_map_)[id] = actor; return (true); }
template <typename PointT> bool PCLVisualizer::fromHandlersToScreen ( const GeometryHandlerConstPtr &geometry_handler, const PointCloudColorHandler<PointT> &color_handler, const std::string &id, int viewport) { if (!geometry_handler->isCapable ()) { PCL_WARN ("[fromHandlersToScreen] PointCloud <%s> requested with an invalid geometry handler (%s)!\n", id.c_str (), geometry_handler->getName ().c_str ()); return (false); } if (!color_handler.isCapable ()) { PCL_WARN ("[fromHandlersToScreen] PointCloud <%s> requested with an invalid color handler (%s)!\n", id.c_str (), color_handler.getName ().c_str ()); return (false); } vtkSmartPointer<vtkPolyData> polydata; vtkSmartPointer<vtkIdTypeArray> initcells; // Convert the PointCloud to VTK PolyData convertPointCloudToVTKPolyData (geometry_handler, polydata, initcells); // use the given geometry handler polydata->Update (); // Get the colors from the handler vtkSmartPointer<vtkDataArray> scalars; color_handler.getColor (scalars); polydata->GetPointData ()->SetScalars (scalars); // Create an Actor vtkSmartPointer<vtkLODActor> actor; createActorFromVTKDataSet (polydata, actor); // Add it to all renderers addActorToRenderer (actor, viewport); // Save the pointer/ID pair to the global actor map (*cloud_actor_map_)[id].actor = actor; (*cloud_actor_map_)[id].cells = initcells; (*cloud_actor_map_)[id].geometry_handlers.push_back (geometry_handler); return (true); }
template <typename PointT> bool pcl::visualization::PCLHistogramVisualizer::addFeatureHistogram ( const pcl::PointCloud<PointT> &cloud, const std::string &field_name, const int index, const std::string &id, int win_width, int win_height) { if (index < 0 || index >= cloud.points.size ()) { PCL_ERROR ("[addFeatureHistogram] Invalid point index (%d) given!\n", index); return (false); } // Get the fields present in this cloud std::vector<sensor_msgs::PointField> fields; // Check if our field exists int field_idx = pcl::getFieldIndex<PointT> (cloud, field_name, fields); if (field_idx == -1) { PCL_ERROR ("[addFeatureHistogram] The specified field <%s> does not exist!\n", field_name.c_str ()); return (false); } RenWinInteractMap::iterator am_it = wins_.find (id); if (am_it != wins_.end ()) { PCL_WARN ("[addFeatureHistogram] A window with id <%s> already exists! Please choose a different id and retry.\n", id.c_str ()); return (false); } vtkSmartPointer<vtkDoubleArray> xy_array = vtkSmartPointer<vtkDoubleArray>::New (); xy_array->SetNumberOfComponents (2); xy_array->SetNumberOfTuples (fields[field_idx].count); // Parse the cloud data and store it in the array double xy[2]; for (int d = 0; d < fields[field_idx].count; ++d) { xy[0] = d; //xy[1] = cloud.points[index].histogram[d]; float data; memcpy (&data, (const char*)&cloud.points[index] + fields[field_idx].offset + d * sizeof (float), sizeof (float)); xy[1] = data; xy_array->SetTuple (d, xy); } RenWinInteract renwinint; createActor (xy_array, renwinint, id, win_width, win_height); // Save the pointer/ID pair to the global window map wins_[id] = renwinint; #if ((VTK_MAJOR_VERSION == 5) && (VTK_MINOR_VERSION <= 4)) resetStoppedFlag (); #endif return (true); }
/** * Saves a mesh into the specified file and output type. The file format is automatically parsed. * @param input[in] The mesh to be saved * @param output_file[out] The output file to be written * @param output_type[in] The output file type * @return True on success, false otherwise. */ bool saveMesh (pcl::PolygonMesh& input, std::string output_file, int output_type) { if (boost::filesystem::path (output_file).extension () == ".obj") { if (output_type == BINARY || output_type == BINARY_COMPRESSED) PCL_WARN ("OBJ file format only supports ASCII.\n"); //TODO: Support precision //FIXME: Color is lost during conversion (OBJ supports color) PCL_INFO ("Saving file %s as ASCII.\n", output_file.c_str ()); if (pcl::io::saveOBJFile (output_file, input) != 0) return (false); } else if (boost::filesystem::path (output_file).extension () == ".pcd") { if (!input.polygons.empty ()) PCL_WARN ("PCD file format does not support meshes! Only points be saved.\n"); pcl::PCLPointCloud2::Ptr cloud = boost::make_shared<pcl::PCLPointCloud2> (input.cloud); if (!savePointCloud (cloud, output_file, output_type)) return (false); } else // PLY, STL and VTK { if (output_type == BINARY_COMPRESSED) PCL_WARN ("PLY, STL and VTK file formats only supports ASCII and binary output file types.\n"); if (input.polygons.empty() && boost::filesystem::path (output_file).extension () == ".stl") { PCL_ERROR ("STL file format does not support point clouds! Aborting.\n"); return (false); } PCL_INFO ("Saving file %s as %s.\n", output_file.c_str (), (output_type == ASCII) ? "ASCII" : "binary"); if (!pcl::io::savePolygonFile (output_file, input, (output_type == ASCII) ? false : true)) return (false); } return (true); }
template <typename PointSource, typename PointTarget, typename NormalT, typename Scalar> bool pcl::registration::CorrespondenceEstimationNormalShooting<PointSource, PointTarget, NormalT, Scalar>::initCompute () { if (!source_normals_) { PCL_WARN ("[pcl::registration::%s::initCompute] Datasets containing normals for source have not been given!\n", getClassName ().c_str ()); return (false); } return (CorrespondenceEstimationBase<PointSource, PointTarget, Scalar>::initCompute ()); }
template <typename PointSource, typename PointTarget, typename NormalT> bool pcl::registration::CorrespondenceEstimationBackProjection<PointSource, PointTarget, NormalT>::initCompute () { if (!source_normals_ || !target_normals_) { PCL_WARN ("[pcl::%s::compute] Datasets containing normals for source/target have not been given!\n", getClassName ().c_str ()); return (false); } return (CorrespondenceEstimation<PointSource, PointTarget>::initCompute ()); }
void ensensoExceptionHandling (const NxLibException &ex, std::string func_nam) { PCL_ERROR ("%s: NxLib error %s (%d) occurred while accessing item %s.\n", func_nam.c_str (), ex.getErrorText ().c_str (), ex.getErrorCode (), ex.getItemPath ().c_str ()); if (ex.getErrorCode () == NxLibExecutionFailed) { NxLibCommand cmd (""); PCL_WARN ("\n%s\n", cmd.result ().asJson (true, 4, false).c_str ()); } }
pcl::FieldComparison<PointT>::FieldComparison ( std::string field_name, ComparisonOps::CompareOp op, double compare_val) : compare_val_(compare_val), point_data_(NULL) { field_name_ = field_name; op_ = op; // Get all fields std::vector<sensor_msgs::PointField> point_fields; // Use a dummy cloud to get the field types in a clever way PointCloud<PointT> dummyCloud; pcl::getFields (dummyCloud, point_fields); // Find field_name if (point_fields.empty ()) { PCL_WARN ("[pcl::FieldComparison::FieldComparison] no fields found!\n"); capable_ = false; return; } // Get the field index size_t d; for (d = 0; d < point_fields.size (); ++d) { if (point_fields[d].name == field_name) break; } if (d == point_fields.size ()) { PCL_WARN ("[pcl::FieldComparison::FieldComparison] field not found!\n"); capable_ = false; return; } uint8_t datatype = point_fields[d].datatype; uint32_t offset = point_fields[d].offset; point_data_ = new PointDataAtOffset<PointT>(datatype, offset); capable_ = true; }
/** @brief Main function * @param[in] argc * @param[in] argv * @return Exit status */ int main (int argc, char *argv[]) { if (argc != 2) { PCL_ERROR ("Usage:\n%s 192.168.100.65\n", argv[0]); return (-1); } viewer_ptr.reset (new CloudViewer ("davidSDK 3D cloud viewer")); davidsdk_ptr.reset (new pcl::DavidSDKGrabber); davidsdk_ptr->connect (argv[1]); if (!davidsdk_ptr->isConnected ()) return (-1); PCL_WARN ("davidSDK connected\n"); #ifndef _WIN32// || _WIN64 PCL_WARN ("Linux / Mac OSX detected, setting local_path_ to /var/tmp/davidsdk/ and remote_path_ to \\\\m6700\\davidsdk\\\n"); davidsdk_ptr->setLocalAndRemotePaths ("/var/tmp/davidsdk/", "\\\\m6700\\davidsdk\\"); #endif //davidsdk_ptr->setFileFormatToPLY(); std::cout << "Using " << davidsdk_ptr->getFileFormat () << " file format" << std::endl; boost::function<void (const PointCloudXYZ::Ptr&)> f = boost::bind (&grabberCallback, _1); davidsdk_ptr->registerCallback (f); davidsdk_ptr->start (); while (!viewer_ptr->wasStopped ()) { boost::this_thread::sleep (boost::posix_time::seconds (20)); std::cout << "FPS: " << davidsdk_ptr->getFramesPerSecond () << std::endl; } davidsdk_ptr->stop (); return (0); }
int main (int argc, char** argv) { if (pcl::console::find_switch (argc, argv, "--help") || pcl::console::find_switch (argc, argv, "-h")) return print_help (); //Reading input cloud pcl::PointCloud<pcl::PointXYZI>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZI>); if (argc < 2) { PCL_ERROR ("No pcd file to read... Exiting...\n"); print_help (); return (-1); } if (pcl::io::loadPCDFile<pcl::PointXYZI> (argv[1], *cloud) == -1) //* load the file { PCL_ERROR ("Couldn't read file %s \n", argv[1]); print_help (); return (-1); } // Creating world model object pcl::kinfuLS::WorldModel<pcl::PointXYZI> wm; //Adding current cloud to the world model wm.addSlice (cloud); std::vector<pcl::PointCloud<pcl::PointXYZI>::Ptr> clouds; std::vector<Eigen::Vector3f> transforms; //Get world as a vector of cubes wm.getWorldAsCubes (pcl::device::kinfuLS::VOLUME_X, clouds, transforms, 0.025); // 2.5% overlapp (12 cells with a 512-wide cube) //Creating the standalone marching cubes instance float volume_size = pcl::device::kinfuLS::VOLUME_SIZE; pcl::console::parse_argument (argc, argv, "--volume_size", volume_size); pcl::console::parse_argument (argc, argv, "-vs", volume_size); PCL_WARN ("Processing world with volume size set to %.2f meters\n", volume_size); pcl::gpu::kinfuLS::StandaloneMarchingCubes<pcl::PointXYZI> m_cubes (pcl::device::kinfuLS::VOLUME_X, pcl::device::kinfuLS::VOLUME_Y, pcl::device::kinfuLS::VOLUME_Z, volume_size); //~ //Creating the output //~ boost::shared_ptr<pcl::PolygonMesh> mesh_ptr_; //~ std::vector< boost::shared_ptr<pcl::PolygonMesh> > meshes; m_cubes.getMeshesFromTSDFVector (clouds, transforms); PCL_INFO ("Done!\n"); return (0); }
template <typename PointSource, typename PointTarget> inline void pcl::Registration<PointSource, PointTarget>::align (PointCloudSource &output, const Eigen::Matrix4f& guess) { if (!initCompute ()) return; if (!target_) { PCL_WARN ("[pcl::%s::compute] No input target dataset was given!\n", getClassName ().c_str ()); return; } // Resize the output dataset if (output.points.size () != indices_->size ()) output.points.resize (indices_->size ()); // Copy the header output.header = input_->header; // Check if the output will be computed for all points or only a subset if (indices_->size () != input_->points.size ()) { output.width = (int) indices_->size (); output.height = 1; } else { output.width = input_->width; output.height = input_->height; } output.is_dense = input_->is_dense; // Copy the point data to output for (size_t i = 0; i < indices_->size (); ++i) output.points[i] = input_->points[(*indices_)[i]]; // Set the internal point representation of choice if (point_representation_) tree_->setPointRepresentation (point_representation_); // Perform the actual transformation computation converged_ = false; final_transformation_ = transformation_ = previous_transformation_ = Eigen::Matrix4f::Identity (); // Right before we estimate the transformation, we set all the point.data[3] values to 1 to aid the rigid // transformation for (size_t i = 0; i < indices_->size (); ++i) output.points[i].data[3] = 1.0; computeTransformation (output, guess); deinitCompute (); }
template<typename PointT> typename pcl::registration::LUM<PointT>::Vertex pcl::registration::LUM<PointT>::addPointCloud (PointCloudPtr cloud, const Eigen::Vector6f &pose) { Vertex v = add_vertex (*slam_graph_); (*slam_graph_)[v].cloud_ = cloud; if (v == 0 && pose != Eigen::Vector6f::Zero ()) { PCL_WARN("[pcl::registration::LUM::addPointCloud] The pose estimate is ignored for the first cloud in the graph since that will become the reference pose.\n"); (*slam_graph_)[v].pose_ = Eigen::Vector6f::Zero (); return (v); } (*slam_graph_)[v].pose_ = pose; return (v); }
template <typename PointT> void pcl::getPointCloudDifference ( const pcl::PointCloud<PointT> &src, const pcl::PointCloud<PointT> &, double threshold, const boost::shared_ptr<pcl::search::Search<PointT> > &tree, pcl::PointCloud<PointT> &output) { // We're interested in a single nearest neighbor only std::vector<int> nn_indices (1); std::vector<float> nn_distances (1); // The src indices that do not have a neighbor in tgt std::vector<int> src_indices; // Iterate through the source data set for (int i = 0; i < static_cast<int> (src.points.size ()); ++i) { if (!isFinite (src.points[i])) continue; // Search for the closest point in the target data set (number of neighbors to find = 1) if (!tree->nearestKSearch (src.points[i], 1, nn_indices, nn_distances)) { PCL_WARN ("No neighbor found for point %zu (%f %f %f)!\n", i, src.points[i].x, src.points[i].y, src.points[i].z); continue; } if (nn_distances[0] > threshold) src_indices.push_back (i); } // Allocate enough space and copy the basics output.points.resize (src_indices.size ()); output.header = src.header; output.width = static_cast<uint32_t> (src_indices.size ()); output.height = 1; //if (src.is_dense) output.is_dense = true; //else // It's not necessarily true that is_dense is false if cloud_in.is_dense is false // To verify this, we would need to iterate over all points and check for NaNs //output.is_dense = false; // Copy all the data fields from the input cloud to the output one typedef typename pcl::traits::fieldList<PointT>::type FieldList; // Iterate over each point for (size_t i = 0; i < src_indices.size (); ++i) // Iterate over each dimension pcl::for_each_type <FieldList> (NdConcatenateFunctor <PointT, PointT> (src.points[src_indices[i]], output.points[i])); }
template <typename PointT> bool pcl::visualization::PCLHistogramVisualizer::updateFeatureHistogram ( const pcl::PointCloud<PointT> &cloud, const std::string &field_name, const int index, const std::string &id) { if (index < 0 || index >= cloud.points.size ()) { PCL_ERROR ("[updateFeatureHistogram] Invalid point index (%d) given!\n", index); return (false); } // Get the fields present in this cloud std::vector<sensor_msgs::PointField> fields; // Check if our field exists int field_idx = pcl::getFieldIndex<PointT> (cloud, field_name, fields); if (field_idx == -1) { PCL_ERROR ("[updateFeatureHistogram] The specified field <%s> does not exist!\n", field_name.c_str ()); return (false); } RenWinInteractMap::iterator am_it = wins_.find (id); if (am_it == wins_.end ()) { PCL_WARN ("[updateFeatureHistogram] A window with id <%s> does not exists!.\n", id.c_str ()); return (false); } RenWinInteract* renwinupd = &wins_[id]; vtkSmartPointer<vtkDoubleArray> xy_array = vtkSmartPointer<vtkDoubleArray>::New (); xy_array->SetNumberOfComponents (2); xy_array->SetNumberOfTuples (fields[field_idx].count); // Parse the cloud data and store it in the array double xy[2]; for (int d = 0; d < fields[field_idx].count; ++d) { xy[0] = d; //xy[1] = cloud.points[index].histogram[d]; float data; memcpy (&data, (const char*)&cloud.points[index] + fields[field_idx].offset + d * sizeof (float), sizeof (float)); xy[1] = data; xy_array->SetTuple (d, xy); } reCreateActor (xy_array, renwinupd, cloud.fields[field_idx].count - 1); return (true); }